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The  experimental  arm  is  driven  by  a  hydraulic  actuator  and 
moves  in  a  vertical  plane.  Hydraulic  actuator  dynamics  and  the 
effects  of  gravity  are  included  in  the  equations  of  motion. 
Computer  simulation  for  the  experimental  arm  is  performed  by  the 
Dynamic  Simulation  Language  (DSL) .  The  validation  of  the 
dynamic  model  includes  the  comparison  between  the  actual  tip 
position  and  the  predicted  arm  tip  position. 
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THESIS  DISCLAIMER 
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ensure  that  the  programs  are  free  of  computational  and  logic 
errors,  they  cannot  be  considered  completely  validated.  Any 
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1.  INTRODUCTION 


A.  MOTIVATION 

As  the  Navy  approaches  the  21st  century,  the  potential  for 
robotic  technology  is  increasing.  Present  and  possible  future 
applications  range  from  industrial  welding  to  shipboard  firefighting, 
from  undersea  exploration  and  salvage  to  weapons  handling.  [Ref. 
1:  pp.  11-14]  All  offer  the  promise  of  freeing  the  sailor  from 
boring,  repetitive  tasks  or,  more  importantly,  making  hazardous  jobs 
much  safer.  Although  Naval  applications  will  most  likely  involve 
some  redesign  or  adaptation  from  their  civilian  counterpart  in 
order  to  meet  the  demands  of  an  at-sea  environment,  it  is 
necesary  to  keep  abreast  of  the  technological  advancements  as  they 
occur.  Everett  [Ref.  l]  presents  and  excellent  overview  of  current 
development  efforts  and  in  the  process  shows  robotics  to  be  an 
inevitable  part  of  the  Navy’s  future. 


B.  NATURE  OF  THE  PROBLEM 

Earlier  work  in  robotics  concentrated  strictly  on  a  rigid  body 
model  of  a  manipulator  system.  Determination  of  the  equations 
of  motion,  inverse  dynamics,  and  end-effector  control  are  examples 
of  the  types  of  problems  that  have  been  solved  for  the  rigid  body 
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case  [Ref.  2:  p.ll].  However,  this  rigid  body  assumption  led  to 
manipulators  that  are  characterized  by  their  large  size,  both  in 
cross-sectional  area  and  weight,  slow  operational  speed  and  low 
payload  to  total  weight  ratio. 

The  demand  for  higher  speeds  and  better  system  performance 
have  caused  the  investigation  of  lightweight  designs,  most  notably  in 
space  applications  [Ref.  3:  pp.3,9].  The  advantages  of  the 

lightweight  design  include  increase  the  speed  of  operation,  and  the 
ease  of  transportability,  decreased  energy  consumption,  and  lowered 
overall  cost  [Ref.  4:  p.167]. 

However,  all  of  these  advantages  are  not  without  cost.  The 
lightweight  design  requires  that  the  flexibility  effects  should  not  be 
neglected.  The  degradation  of  end-effector  positioning  accuracy 
occurs  if  the  increased  deformations  due  to  bending  and  torsion 
effects  of  the  lightweight, flexible  manipulator  are  not  modeled. 
(Flexibility  must  be  taken  into  account  since  actuators  which 
actuate  rigid  motion  also  excite  deformations.) 

Additional  information  is  needed  regarding  the  dynamics  of  the 
flexible  manipulator  so  that  the  system  can  be  designed  to 
withstand  loads  without  exceeding  fatigue  and  bending  stress  limits. 
In  addition,  system  natural  frequencies  are  needed  for  control  and 
sampling  considerations  [Ref.  5:  p.  2].  A  method  for  dynamic 
modeling  of  manipulators  with  flexible  members  is  needed  for  the 
design  of  controllers.  This  model  must  be  able  to  accurately 
predict  system  responses,  be  able  to  be  efficiently  simulated  and  be 
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computationally  effective  so  that  accurate  motion  control  can  be 
achieved . 


C.  LITERATURE  REVIEW 

Research  into  the  design  and  control  of  flexible  manipulators 
began  in  the  early  to  mid  1970's.  The  efforts  centered  around  the 
design, modeling  and  control  of  a  planar  two  link  manipulator. 
Since  then,  many  approaches  have  been  taken  to  the  development 
of  an  accurate  dynamic  model. 

Most  models  involve  an  attempt  to  find  the  relationship 
between  the  large,  rigid-body  motion  (large  motion)  and  the  small 
motion  displacements  (small  motion)  arising  from  the  flexibility  of 
the  structure. 

Book  [Ref.  6]  used  the  4  *  4  transformation  matrices, common 
to  rigid  body  models,  to  model  the  elastic  chain.  This  enabled  the 
representation  of  spatial  motion  of  a  manipulator  with  an  arbitrary 
number  of  links  and  joints.  His  model  was  limited  in  the 
assumption  that  the  mass  of  the  manipulator  is  negligible  compared 
to  the  mass  of  the  load.  This  is  acceptable  in  space  applications 
where  the  manipulator  is  very  light  and  flexible,  but  not  for 
industrial  applications  where  the  effects  of  mass  must  be 
considered . 

Sunada  and  Dubowsky  [Ref .  7]  similiarly  expressed  the 

kinematics  and  dynamics  of  the  manipulator  in  terms  of  4  *  4 
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transformation  matrices.  Lagrangian  formulation  with  finite 
element  methods  was  used  to  model  the  system  with  flexibility 
effects.  A  dynamic  coordinate  reduction  procedure.  Component 
Mode  Synthesis,  was  used  to  reduce  the  number  of  governing 
equations  improving  computational  efficiency.  Manipulator  motion 
was  expressed  as  "small  perturbations  about  some  known  nominal 
motion  of  the  system".  This  small  motion  superposition  onto  an 
assumed  nominal  large  motion  neglects  the  effects  of  the  small 

motion  interaction  on  the  large  motion;  in  turn,  the  consideration 
of  the  coupling  effect  was  not  complete. 

Book  [Refs.  8,9]  also  includes  small  motion  displacements  in 
the  4  *  4  transformation  matrices.  He  used  a  modal  approach 

to  model  system  flexible  kinematics,  truncating  the  series  of 
assumed  vibration  modes.  After  applying  recursive  Lagrangian 
dynamics  and  a  hybrid  set  of  large  and  small  motion  coordinates, 
the  dynamic  equations  of  motion  were  obtained.  Although  an 

accurate  model  describing  large  motion,  small  motion  and  their 
coupling,  the  resulting  set  of  coupled,  non-linear  second  order 

ordinary  differential  equations  is  computationally  intensive 
and,  therefore,  expensive  to  solve. 

Truckenbrodt  [Ref.  10]  developed  a  model  for  a  six  degree  of 
freedom  manipulator  with  two  flexible  links.  The  flexible 
manipulator  was  modeled  as  a  "hybrid  multibody  system"  having 
both  rigid  and  flexible  elements.  The  flexible  coordinates  were 
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represented  using  truncated  series  expansion.  The  model  was  then 
linearized  for  control  purposes. 

Cannon  and  Schmitz  [Ref.  ll]  performed  experimental  studies 
on  a  "very  flexible"  single  link  manipulator.  Their  model  was  also 
truncated  and  linearized.  A  simplified  analytical  model  was  used 
to  derive  the  general  form  of  the  equations  of  motion. 
Experimental  analysis  was  then  used  to  determine  the  coefficients. 

Chang  [Ref.  5]  introduced  the  Equivalent  Rigid  Link  System 
(ERLS)  dynamic  model  of  a  flexible  manipulor.  Global  motions 
were  separated  into  large  motions  and  small  motions.  The  ERLS 
described  the  kinematics  of  the  large  motion.  The  small  motion 
displacements  were  described  relative  to  the  ERLS.  The  finite 
element  method  was  used  to  discretize  deformations  and 
Lagrangian  formulation  was  used  to  derive  the  equations  of 
motion.  Two  sets  of  coupled,  non-linear  ordinary  differential 
equations-large  motion  equations  and  small  motion  equations 
resulted.  The  set  of  large  motion  equations  were  non-linear  in 
both  the  large  and  small  motion  variables.  The  set  of  small 
motion  equations  were  linear  in  the  small  motion  variable  but 
non-linear  in  the  large  motion  variable.  Chang  also  developed  a 
solution  technique  called  the  Sequential  Integration  Method.  It 
allows  simulations  of  systems  with  inertia  coupled  motions  having 
non-linear  slow  motion  (large  motion)  with  linear  fast  motion 
(small  motion) .  Chang’s  model  presented  a  complete,  efficient 
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dynamic  model  being  able  to  describe  large  motion,  small  motion 
and  their  coupling. 

Petroka  [Ref.  12]  performed  an  experimental  validation  of 
Chang’s  dynamic  model.  He  designed  a  hydraulically  actuated, 
single-link  flexible  arm  and  applied  the  ERLS  model.  He  tailored 
the  model  for  a  single  link  flexible  manipulator  with  vertical  plane 
motion.  Finite  element  analysis  was  limited  by  single  element 
modeling  and  the  choice  of  a  simple  cubic  function  representation 
for  the  shape  function.  Tip  position  information  was  determined 
from  motion  picture  studies,  additional  data  was  obtained  from  a 
comparison  between  theoretical  and  experimental  strain.  Results  of 
the  validation  suggested  the  potential  usefulness  of  the  model  in 
determining  tip  position. 


D.  PURPOSE 

The  purpose  of  this  study  is  to  perform  a  computer  simulation 
and  experimental  validation  of  a  dynamic  model  including 
flexibility.  Chang's  Equivalent  Rigid  Link  System  dynamic  model 
using  an  enhanced  natural-mode  discretization  is  chosen.  The 
research  also  serves  as  a  continuation  of  the  experimental  work 
performed  on  the  single-link  flexible  manipulator  designed  by 
Petroka.  Computer  simulation  for  the  experimental  arm  is 
performed  by  the  Dynamic  Simulation  Language  (DSL)  on  the  IBM 
3033  mainframe  computer.  A  piezo- resistive  accelerometer  and 
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strain  gauges  are  used  for  tip  position  and  strain  information 
Data  acquisition  was  performed  using  a  GWI  Instruments  MacAdios 
hardware  and  software  data  acquisition  system  in  conjunction  with 
a  Macintosh  512k  computer. 

An  understanding  of  the  Equivalent  Rigid  Link  System  model 
is  essential.  The  theory  and  application  behind  the  model, its 
adaptation  to  the  single  link  case,  the  natural-mode 
discretization,  and  a  discussion  of  the  computer  simulation,  are 
discussed  in  Chapter  11. 

A  description  of  the  experimental  apparatus,  design  and 
procedure  are  reviewed  in  Chapter  111.  Data  acquisition  is  also 
discussed. 

Chapter  IV  presents  the  results  of  the  validation. 
Comparisons  are  made  between  experimental  arm-tip  position, 
theoretical  arm-tip  position  using  cubic  shape  functions,  and 
theoretical  arm-tip  position  using  natural-mode  discretization. 
Also  a  comparison  is  made  between  experimental  strain  and  the 
theoretical  strain  predicted  by  the  ERLS  dynamic  model 
simulation. 

Chapter  V  is  devoted  to  conclusions  reached  during  the  study 
and  to  recommendations  for  future  work  in  the  area  of  flexible 
manipulators. 
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II.  MODEL  FORMULATION  FOR  A  SINGLE-LINK  FLEXIBLE 


MANIPULATOR 

A.  EQUIVALENT  RIGID  LINK  SYSTEM 

The  ERLS  dynamic  model  of  a  flexible  manipulator  describes 
the  large  rigid-body  motion  of  the  manipulator  and  the  small- 
motion  displacement  arising  from  the  flexibility  of  the  structure. 
A  schematic  of  the  ERLS  is  shown  in  Fig.  2.1.  The  ERLS 
describes  the  large  motion  of  the  manipulator.  A  strength  of  the 
ERLS  is  that  the  description  of  kinematics  is  equivalent  to  the  rigid 
manipulator  system  so  that  the  the  familiar  mathematical  tools  for 
the  rigid  system  can  be  used  to  solve  flexible  manipulator 
problems.  The  small-motion  displacements  are  described  relative 
to  the  ERLS.  Displacements  are  for  each  point  along  the  flexible 
arm,  a  function  of  location  and  time,  so  it  is  necessary  to  discretize 
the  deformations  for  digital  computation.  The  techniques  of  the 
Finite  Element  Method  (FEM)  are  utilized  for  this  discretization. 
In  this  study,  an  enhanced  natural-mode  discretization  which 
represents  the  small  motion  in  terms  of  nodal  displacements  and 
the  natural  modes  of  the  structure  is  introduced. 

After  describing  the  kinematic  relationships  between  the  large 
and  small  motions,  kinetics  is  introduced  to  complete  the 
derivation  of  the  equations  of  motion.  The  Lagrangian  dynamics 
approach  is  used  for  the  derivation  due  to  its  straightforward  and 
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V 


Figure  2.1  Equivalent  Rigid  Link  System  (ERLS) 


systematic  nature  which  is  helpful  in  the  analysis  of  complex 
systems.  This  formulation  requires  generalized  coordinates.  Since 
the  global  motions  are  separated  into  large  motions  and  small 
motions,  generalized  coordinates  are  defined  to  represent  these  two 
motions.  The  three  generalized  coordinates  chosen  are  the  rigid 
body  rotation,  0,  and  two  nodal  displacements,  v(0)  and  <j>(0],  which 

can  be  measured  at  the  end  of  the  link.  Kinetic  energy  of  the 
system  is  due  to  kinetic  energy  of  the  link,  kinetic  energy  of  the 
actuator  and  the  kinetic  energy  of  any  load.  The  actuator  is 
treated  as  a  rigid  body  and  all  kinetic  energy  terms  are  calculated 
separately.  Potential  energy  of  the  system  comes  from  strain 
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energy  due  to  deformation  and  from  gravity.  Generalized  forces 
include  any  applied  forces  and  damping  forces.  The  actual 
derivation  of  the  equations  of  motion  requires  detailed,  tedious 
mathematical  manipulations,  and  the  Lagrangian  formulation  yields 
two  sets  of  equations.  One  set  describes  the  large  motions  and 
the  other  set  describes  the  small  motions.  These  two  sets  of 
equations  are  non-linear,  coupled,  second-order,  ordinary 
differential  equations  represented  as  follows, 


e  +  hi  U  -  F 
qq  q 


(2-1) 
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deformations  representing  the  small  motions 
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A  more  detailed  derivation  of  the  equations  of  motion  is 
included  in  Appendix  A. 


B.  SHAPE  FUNCTION  DERIVATION 

In  Petroka’s  study  a  cubic  shape  function  was  assumed  to 
represent  the  transverse  displacement  of  the  flexible  manipulator.- 
This  allowed  a  complete  representation  of  the  displacement 
including  rigid  body  rotation  and  translation.  In  this  study  the 
natural-mode  shape  functions  of  a  beam  are  used  to  represent 
the  flexural  motion  of  the  single-link  flexible  arm.  Only  the  first 
two  mode  shapes  are  used.  The  flexible  manipulator  arm  is 
modeled  as  a  continuous  Euler- Bernoulli  cantilever  beam, 
neglecting  shear  deformation, and  rotary  effects.  The  beam  has 
the  following  characteristics. 


p.-mass/unit  length 
EI=flexural  rigidity 
M-moment 
3M/3x -  V 

Incurvature  =  M/EI-32z/3x2 
(linear  beam  theory) 


L=  length 

p=transverse  load/unit  length 
V  -  shear  force 
3V/3x - p 

+  (3y/3x]2}3/2  *  32z/3x2 


This  allows  us  to  express  p  in  the  following  form, 
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p  -  d2 M/dx2  -  d2[El  cPv/dxtVdx2  -  -p  dPv/dl2  (2-3) 

Thus,  for  a  uniform  beam, 

El  tfv/dx4  -  -  p.  a^/at2  (2-4) 

Let  vlx,tl  -  Xtxl  sin  cot,  substituting  into  Eq.  (2-4), 

El  XIV  sin  cot «  -  p.  co2  X  sin  cot 

therefore, 

X1V  -  p4  X  where  p4  -  Ji  co2/El  (2-5) 

The  above  equation  has  the  general  solution, 

X  -  ft  cosh  px  ♦  B  sinh  px  +  C  cos  px  +  D  sin  px  (2-6) 

For  the  cantilever  beam  model  the  boundary  conditions  require 
that, 

at  x  -  -  L  (.base],  X  Hl-0  and  /  Hl-0  (2-7) 

at  k-0  [tipi  X"[01-0  and  X'"  [01  - 0  (2-8) 

Rewriting  Eq.  (2-6)  in  the  form, 

X  -  ft  [  cos  px  +  cosh  px  1  +  B  [  cos  px  -  cosh  px  1 

♦  C  [  sin  px  +  sinh  px  1  +  D  l  sin  px  -  sinh  px  1  (2-9) 

shows  that,  (2-7)  implies  B=D=0,  consequently, 

X  -  ft  [  cos  px  +  cosh  px  )  ♦  C  [  sin  px  *  sinh  px  1  (2-10) 
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Using  the  boundary  conditions  from  Eq.  (2-7)  and  substituting 
into  Eq.  (2-10)  and  solving  yields: 

cosh  0L  cos  0L  --1  (2-11) 

The  first  three  consecutive  roots  are: 

p,L  -  1.875104069 
fcL  -  4.694091133 
03L  -  7.854757438 

•  Equation  (2-10)  is  now  written  in  the  form: 

X  *  ft /  (  cos  0x  +  cosh0x  ]  +  (  sin  0x  +  sinh  0x]  (2-12) 

where  ft,'  -  sin  pi  ±  Sinh  pi  .  .  -  ft/C 

cos  0.L  ♦  cosh  0.L 

This  form  satisfies  Eqs.  (2-7),  (2-8), and  (2-11)  and  will  be 
the  form  used  in  further  derivations. 

The  transverse  displacement,  and  slope,  of  the  single  link 
flexible  arm  can  now  be  represented  in  the  following  forms, 
respectively, 

vlXjt)  =  a i  (t.)  X]  (x]  +  a2  ft)  Xj  (x)  (2-13) 

=  a,  (  A,"  (  cos  0,  x  +  cosh  0,  x  )  ♦  (  sin  0,  x  ♦  sinh  0,  x)) 

*  a2(  ft2/  l  cos  02x  *  COSh  t  sin  fox  *  Sinh  02X)) 

<j>  -  a,  ( ft,  'fa  [  -sin  0,  x  ♦  sinh  0,  x  )  ♦  0,  ( cos  0tx  ♦  cosh  0,  x))  (2-14) 
+  a2 1 82  1  02 1  -sin  02X  +sinh  02X  ]  +  02(  cos  02X  +  cosh  02X  )} 
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Substituting  the  boundary  conditions  into  the  shape  functions 


gives, 

vlO)  -  2  a,  (t)  fl, '  +  2  a2  ft]  (2-15) 

4>lO)-2a1(t]p,  ♦  2  a2lt]  fc.  (2-16) 

v(-L]  -  0  (2-17) 

4>[-U  -  0  (2-18) 


Solving  the  two  equations  for  aj  and  a2  gives: 

ajX) -  2  p2  vfO)  -  2  fW  *101  a^t)  =  -2  fovfOl  *2Pi{  <bfQl  (2-19) 

4  fl|' p2  -  4  fl2' Pi  4  fl|'  p2  -  4  fi2'  Pi 


For  ease  of  manipulation,  make  the  following  substitutions: 


C|-2  p2  /  t4  P2~4A2  Pi)  (2— 20) 

C2  — 2p|/[4flf  p2-4A2  Pi)  (2-21) 

C3  -  -  2  A2  /  (4  ft,'  p2  -  4  f)2  p,]  (2-22) 

C4  -  2  A,'  /  (4  flf  p2  -  4  fy'  Pi )  (2-23) 


Substituting  Eqs.  (2-20),  (2-21),  (2-22) ,  and  (2-23)  into  Eq. 
(2-13)  and  collecting  terms  yields  an  expression  for  the  transverse 
displacement  of  the  single-link  flexible  arm  as  a  function  of  the 
arm  tip  nodal  displacements,  v(0]  and  4>(0]: 

vfx,t]  - 1  Cj  l  flf  l  cos  P|X  +  cosh  P|X)  ♦  [  sinp|X  +  sinh  pjx))  (2-24) 
+  C2  [  f)2/ t  cos  P2X  +  cosh  P2X)  +  l  sin  P2X  +  sinh  P2X)))  vl°) 

+ 1 C3  [  A]'  (  cos  Pjx  +  cosh  Pjx)  4-  (  sinpjx  4-  sinh  P|X)} 

4-  (  A2  (  cos  p2x  4-  cosh  p2x)  4-  (  sin  p2x  +  sinh  p2x)])  <J>(0) 
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This  expression  is  differentiated  twice  to  obtain  v'  "(x),  which  is 
necessary  for  the  calculation  of  the  potential  energy  due  to 
deformation  and  the  theoretical  strain, 

(2-25) 

v"(x,t)  =  (  Ci  (3f  (  flf  (  -cos  (3]X  +  cosh  (Six)  +  ( -siripix  +  sinh  pix)) 

+  C2  $'£  (  ft 2  ( "cos  P2X  +  cosh  f?2'X)  +  (  -sin  fo-x  +  sinh  (32x11)  v(0) 

+  (  C3  pi2  (  ft\  ( -cos  (3|X  +  cosh  pix]  +  ( -sinpix  +  sinh  pix)) 

+  C4  |322(  ft 2  l  "COS  ($2X  +  cosh  f32'X)  +  (  "Sin  (32 X  +  Siflh  (32X)))  4>t0) 


Now  substitution  of  v(x)  into  the  3X1  deformation  vector,  d, 
yields  the  3x2  shape  function  matrix,  ,  and  the  2X1  nodal 
displacement  vector,  U: 


d  «  0  U 


& 


0 

0 

_  \ 
u 

v(0) ' 

0 

= 

0 

0 

Mo) 

v(x) 

a 

b 

N  /  N  /  . 


where  a  and  b  are  the  coefficients  of  v(0)  and  4>(Q),  respectively,  in 


Eq..  (2-24),  defined  here,  for  convenience, 

a  =  C|  (  flf  (  cos  (3 |X  +  cosh  (3jx)  +  (  sinpjx  +  sinh  |3jX)l  (2-27) 

+  C2  (  ft 2  (  cos  (32X  +  cosh  (3 2‘X )  +  (  sin  +  sinh  (32x)l 
b  =  C3  (  R{  (  cos  |3|X  +  cosh  (3-jx]  +  (  siri|3|X  +  sinh  fSjx}}  (2-2S) 

+  C4  (  ft 2  {  cos  (S2X  +  cosh  (32X1  +  (  sin  |32X  +  sinh  (32X11 


The  shape  function  matrix  is  now  in  a  form  convenient  for 
computer  coding. 
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C.  HYDRAULIC  ACTUATION 

The  single-link  flexible  manipulator  uses  hydraulic  actuation. 
Therefore,  hydraulic  actuator  and  servovalve  dynamics  are  an 
integral  part  of  the  total  system  model.  Figure  2.2  illustrates  the 
relationship  of  hydraulic  dynamics  to  the  overall  system. 


Figure  2.2.  System  drawing 


The  hydraulic  dynamics  are  represented  by  servovalve 
dynamics  and  actuator  dynamics .  Moog,  the  manfacturer  of  the 
servovalve,  simplified  the  description  of  servovalve  dynamics  to  a 
single  equation  [Ref.  13], 


where 

Q  =  Flow  delivered  from  servovalve 

I  =  Input  current 

K  =  Valve  sizing  constant  contributes  to  hydraulic  system 
damping 

P  v  =  Valve  pressure  drop,  Ps  -  PL 


Actuator  dynamics  consist  of  a 'form  of  the  continuity  eqati  on  (2-30) 
and  the  torque  output  equation  (2-31)  [Ref.  14], 


Q  =  Dm  0  +  Ctrn  PL  +  Vt  PL 

4 


(2-30) 


=  rlt  Pl  d, 


m 


(2-31) 


where 

Q 

0  = 
*-4rn  PL  = 

Vt_?  = 

4  (3e 
Td 

nt 

Pl 

Dm  = 


Flow  delivered  to  actuator 
FIouj  causing  actuator  rotation 
Leakage  -flotu  in  actuator 
Compressibility  flow 

Torque  required  to  overcome  inertia  and  move  the  load 
Torque  efficiency 
Load  pressure  drop 
Motor  displacement 


A  detailed  account  into  the  selection  of  hydraulic  components 
for  the  system  was  included  in  Petroka’s  work. 


D.  COMPUTER  SIMULATION 

Computer  simulation  of  the  equations  of  motion  is  performed 
by  the  Dynamic  Simulation  Language  (DSL)  which  solves  a  set  of 
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simultaneous,  nonlinear,  second- order,  coupled,  ordinary  differential 
equations.  DSL  is  a  differential  equation  solver  that  was  designed 
by  IBM  for  mini  and  mainframe  computers  as  a  follow  on  to 
Continuous  System  Modeling  Program  (CSMP) .  It  was  chosen  for 
its  compatibility  with  Fortran,  good  selection  of  integration  methods 
(Adams  method  chosen),  implicit  double  precision,  powerful 
graphics  capability,  and  ease  of  use. 

The  computer  code  was  adapted  from  Petroka’s  work.  Two 
major  differences  are  Petroka  used  CSMP  and  a  variable 
step, fourth  order, Runge-Kutta  integration  method.  The  decision  to 
use  the  Adams  method  was  based  on  several  trial  runs  of  the 
program.  In  each  run  the  Adams  method  was  significantly  faster 
than  Runge-Kutta,  sometimes  as  high  as  five  times  faster  yet 
maintaining  agreement  of  solution  to  the  fourth  decimal  place. 

The  algorithm  of  solution  involves  substituting  Eqs.  (Z  —  1 )  and 
(2-2)  into  matrix  form  to  create  a  3  '<  3  coefficient  inertia  matrix 
and  a  3  *  1  right  hand  side  vector  of  forces  and  moments.  The 

t  •  •  fl 

unknowns  are  9  and  U  . 

Each  coefficient  is  calculated  in  a  separate  subroutine.  Once 
calculated  the  elements  are  put  into  matrix  form.  An  1MSL  linear 
equation  solver  subroutine  is  then  called  to  solve  for  the 
accelerations.  The  accelerations  are  then  integrated  twice  using 
the  Adams  method  of  the  DSL  system.  A  global  transformation 
subroutine  is  then  called  to  global  arm  tip  position.  The  computer 
code  is  included  as  Appendix  B. 
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III.  EXPERIMENTAL  FLEXIBLE  ARM 


A.  EXPERIMENTAL  APPARATUS 

The  experimental  apparatus  used  to  validate  the  Equivalent- 
Rigid  Link  System  (ERLS)  on  a  single- link  flexible  manipulator 
consisted  of  a  flexible  arm,  a  hydraulic  system,  sensors,  an  analog 
to  digital  converter,  and  a  computer. 

1 .  Flexible  Arm 

The  flexible  arm  shown  in  Figures  3.1.  and  3.2.  is  a  one- 
meter  long  flexible  structure  which  can  bend  freely  in  the  vertical 
plane  but  is  stiff  in  torsion  and  horizontal  bending.  The  arm 

consists  of  two  parallel  steel  flat,  bars  welded  at.  the  base  and 

directly  clamped  to  the  hydraulic  actuator.  Torsional  stiffness  is 
obtained  by  connecting  the  two  steel  beams,  with  thin  steel 
strips,  to  seven  transverse  steel  bridges.  If  the  arm  consisted  of 
only  a  single  steel  beam  it  would  deflect  due  to  any  bending 

moment  and  twist  around  the  beam  neutral  axis.  The  additional 
thin  steel  strips  act  to  increase  the  torsional  rigidity  of  the  beam. 

Table  3 . 1  shows  some  geometric  and  mass  properties  of  the 
flexible  arm. 

2 .  Hydraulic  System 

The  power  for  the  flexible  arm  was  provided  by  a 

hydraulic  system  .  The  system  consisted  of  a  York  hydraulic 
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Figure  3.1.  Single— Link  Flexible  Manipulator  System 
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TABLE  3 .  i 


ARM  PARAMETERS 


Parameter 

Value 

LL=arm  length 

0.9985  rn 

T=beam  thickness 

0.003175  m 

W=beam  width 

0.0762  m 

D= distance  between 
each  beam 

0.05715  m 

MT=arm  total  mass 

4 . 8565  kg 

E=  modulus  of  elasticity 

2.0  ell  N-m2 

p  =  density /unit  volume 

7861.05  kg/m 3 

power  unit  shown  in  Figure  3.3.,  a  Bird- Johnson  3-axis  Hyd-Ro- 
Wrist,  a  Moog  760-100  servovalve,  and  associated  Moog 
servocontroller  and  high  pressure  filter  assembly. 

The  York  hydraulic  power  unit  includes  a  three  horsepower 
motor  and  starter  and  is  capable  of  supplying  a  system  pressure  of 
2250  psi.  Standard  supply  pressure  for  all  runs  was  2000  psi. 

The  actuator  selected  was  a  Bird-Johnson  3-axis  Hyd-Ro- 
Wrist  with  a  displacement  in  the  pitch  axis  of  4.0  in 3  /  rad.  It  is 
a  hydraulic  rotary  actuator  supported  on  roller  bearings  with  180 
degrees  of  rotation.  It  was  chosen  by  Petroka  [Ref.  12]  after  a 
careful  selection  process.  The  principal  consideration  was  a 
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minimum  displacement  of  1.0788  in3  /  rad,  based  on  an  analysis 
of  the  system  using  the  relationship 


-  T„  /  r,t  PL  (2-31) 


The  wrist  also  includes  roll  and  yaw  axis,  but  these  were 
disconnected  and  not  used  for  this  thesis. 

The  Moog  760-100  servovalve  has  a  rated  flow  of  1.0  gprn. 
This  was  chosen  to  satisfy  the  design  flow  rate  of  0.637  gprn. 

The  760  series  servovalve  is  a  two-stage  design.  The  output  stage 

is  a  closed  center,  four- way,  sliding  spool.  The  pilot  stage  is  a 
symmetrical  double-nozzle  and  flapper,  driven  by  a  double  air  gap, 
dry  torque  motor  in  a  sealed  compartment.  Spool  position 
feedback  is  provided  by  a  cantilever  spring. 

3.  Sensors 

The  flexible  manipulator  had  three  principal  sensors-  an 

accelerometer  for  acceleration  ,  strain  gages  for  strain,  and  a 

potentiometer  for  large  motion  angle. 

The  accelerometer  was  an  Endevco  model  7255A  high 

sensitivity,  small  size  (6  grams),  damped,  piesoresistive 
accelerometer.  A  piesoresistive  type  accelerometer  was  chosen  for 
its  good  low  frequency  response,  an  important  consideration  in  our 
case.  The  other  predominant  factor  was  small  size  for  minimum 
mass  loading.  The  damping  helps  to  extend  the  useful  high 
frequency  range  and  reduce  the  effects  of  spurious  high  frequency 
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excitation.  The  accelerometer  was  mounted  at  the  tip  of  the 
flexible  manipulator,  with  the  axis  of  maximum  response  aligned 
along  the  length  of  the  arm.  Signal  conditioning  was  provided  by 
a  BAM-1  amplifier. 

Four  CEA-06-12UW-350  strain  gages  were  attached  to  the 
flexible  manipulator.  Two  were  located  at  the  base  and  two  at 
the  mid- section  of  each  steel  bar,  on  opposite  sides  .of  the  neutral 
axis.  A  mid- length  strain  gage  was  used  for  all  data  runs  since  it 
provided  the  maximum  response.  Signal  conditioning  was  again 
provided  by  the  BAM-1  amplifier. 

A  Bourn  potentiometer  was  used  to  obtain  large  motion 
angle  data.  This  was  essential  information  for  determining  tip 
position  in  terms  of  the  global  coordinate  system. 

The  heart  of  the  data  acquisition  system  was  a  Apple 
Macintosh  512K  computer  coupled  with  a  MacADlOS  analog- to- 
digital/digital- to- analog  convertor.  The  MacADlOS  is  produced  by 
'3  VI  Insrurnents  and  the  hardware  is  called  a  Model  411  Hardware 
Unit.  It  comes  with  its  own  software  and  serial  interface  to  the 
Macintosh.  It  has  15  digital  inputs  and  outputs,  8  analog  inputs 
and  4  analog  outputs,  a  programmable  clock  signal  and  a 
programmable  counter.  The  MacADlOS  can  be  programmed  using 
Microsoft  BASIC  or  C  languages. 

The  strenth  of  the  MacADlOS  system  is  the  wide  array  of 
software  that  is  included  with  the  system.  This  software  includes 
the  5  Instruments  Package,  MacADlOS  Manager,  XY  MaCorder, 


32 


Microsoft  BASIC  library  calls  and  source  code  for  Manx  A 2 tec  C. 
The  5  Instruments  Package  permits  the  system  to  emulate  a  S 
input  digital  voltmeter,  oscilliscope,  spectrum  analyzer,  sonogram, 
and  a  spectrogram. 

The  software  primarily  used  for  the  thesis  was  the 
portion  called  the  Mac  ADI  OS  Manager  (MM)  and  the  library  of 
associated  BASIC  routines.  This  allowed  the  formulation  of  two 
experiments,  one  for  tip  position  determination  using  accelerometer 
information,  and  the  other  for  determining  experimental  strain 
from  strain  gage  data  .  Two  channels  of  analog  data  are  input 
(accelerometer  output  and  potentiometer  output,  or  strain  gage 
output  and  potentiometer  output),  this  limits  sampling  to  a  172  Hz 
rate  or  5.81  milliseconds  per  sample  [Ref.  15],  the  resulting 
waveforms  are  digitally  stored  awaiting  further  processing.  Single 
channel  inputs  can  be  sampled  at  the  much  higher  rate  of  96 
microseconds.  The  raw  data  can  be  graphically  displayed  using 
MM. 


B.  EXPERIMENTAL  PROCEDURE 
1 .  Tip  Position  Determination 

The  hydraulic  power  unit  was  started  and  allowed  to  run 
for  approximately  30  minutes  to  allow  the  system  temperature  to 
stabilize.  During  this  time  the  MacADIOS  was  set  up  to  perform 
as  a  voltmeter  so  that  initial  calibration  of  the  amplifier  and 
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accelerometer  output  could  be  done.  Voltage  to  the  servovalve 
controller  was  set  at  2  volts.  This  produced  an  input  current  of 
4  milliarnps  to  the  servovalve.  This  current  was  used  to  allow 
direct  comparison  with  Petr  oka's  [Ref.  12]  results.  The  Mac  A  DIOS 
Manager  was  setup  to  sample  two  analog  waveforms, 
accelerometer  output  and  potentiometer  output,  at  a  rate  of  6 . 00 
milliseconds,  or  166.67  Hz,  and  store  these  waveforms  in  digital 
form.  This  formatted  the  data  for  later  transportation  via  modem 
to  the  IBM  3033  mainframe  computer  for  data  reduction.  The 
manipulator  was  then  loaded  by  attaching  weights  to  the  4  studs 
at  the  tip  of  the  manipulator.  No  load  ,  2.116  Kg,  and  4.223  Kg 
load  conditions  were  investigated.  Once  the  load  was  attached  to 
the  manipulator,  the  manipulator  was  placed  in  the  horizontal 
position  and  allowed  to  come  to  rest.  From  this  position  a  step 
input  of  4  milliarnps  was  applied  to  the  servovalve.  Data  were 
then  collected  by  the  Mac  ADI  OS. 

2 .  Experimental  Strain  Determination 

The  procedure  for  experimental  strain  determination  was 
very  similiar  to  the  procedure  for  tip  position  determination 
described  above.  The  major  differences  being  that  strain  gage 
output  is  taken  vice  accelerometer  output  and  the  flexible 
manipulator  is  initially  in  the  vertical  position  so  that  the  initial 
strain  in  0.0  micrometers/meter. 
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IV.  RESULTS 


The  experimental  work  was  divided  into  three  areas.  First  a 
comparison  was  made  between  the  two  methods  of  determining  tip 
position,  accelerometer  and  motion  pictures.  These  results  were 
used  as  a  baseline  to  compare  this  thesis  with  past  work.  Then  a 
comparison  was  made  between  experimental  tip  position  and  tip 
position  determined  from  the  two  simulation  programs  one  using  a 
modal  shape  function  the  other  using  a  cubic  shape  function. 
Finally  a  comparison  was  made  between  experimental  and 
theoretical  strain  again  comparing  the  two  theoretical  models  to 
the  experimental  value. 


A 

14.  . 


ACCELEROMETER  VERSUS  MOTION  PICTURES 


One  of  the  primary  objectives  of  this  research  was  to 
investigate  a  different  technique  for  determining  arm  tip  position. 
Petroka  [Ref.  12]  considered  several  methods  for  his  thesis  including 
motion  pictures,  an  automatic  laser  tracking  interferometer 
system,  digitising  tablets,  ultrasonics,  position/displacement 
transducers,  digitising  vision  systems  and  accelerometers  before 
finally  choosing  motion  pictures.  The  principal  factors  in  the 
decision  were  simplicity,  availability,  and  cost.  He  encountered 
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some  limitations-  to  this  method  during  the  course  of  his  thesis. 
These  included  requirement  for  a  slow,  tedious  frame-by-frame 
examination  hindered  by  the  lack  of  clarity  of  the  arm  tip  position 
and  by  the  absence  of  definition  of  the  background  grid 
measurement.  This  method  is  also  limited  in  that  it  cannot  be 
used  to  feedback  position  data  to  a  controller  for  appropriate 
control  action.  . 

The  additional  method  investigated  in  this  thesis  was  the  use  of 
an  accelerometer.  A  piesoresi stive  accelerometer  was  attached  to 
the  end  of  the  flexible  arm.  A  potentiometer  was  attached  to  the 
rotary  hydraulic  actuator  at  the  base  of  the  flexible  manipulator. 
Accelerometer  output  and  potentiometer  output  were  then  used  to 
obtain  tip  position  information.  This  required  two  integrations  and 
a  global  transformation.  The  accelerometer  senses  a  global 
acceleration  we  will  call  S.  This  can  be  related  to  X  and  Y  global 

coordinates.  Using  our  global  transformation  matrix  we  can  then 
obtain  an  expression  for  v(0).  This  is  then  integrated  twice  using  a 

DSL  simulation  program.  Once  again  using  the  global 
transformation  we  can  obtain  an  expression  for  tip  position  in 
terms  of  the  global  coordinate  system.  This  process  is  discussed  in 
further  detail  in  Appendix  C.  Figures  4. 1-4.6  show  the  - 
comparisons  between  tip  position  determined  by  Petroka  using 
motion  pictures  and  by  accelerometer  in  this  thesis.  The  three 
loading  conditions  were  no  load,  a  2.115  kg  load,  and  a  4.253  kg 
load.  For  comparative  purposes,  a  percent  difference  was 


calculated  with  motion  picture  data  representing  the  reference. 
This  is  given  in  Table  4.1.  All  experimental  runs  were  made  with 
a  4  rnilliarnp  step  input  of  current.  The  initial  condition  for  all 
runs  was  the  horizontal  position  of  the  flexible  arm. 

The  figures  and  table  values  indicate  that  there  is  good 
agreement  between  the  two  methods  for  determining  tip  position. 
There  is  closer  agreement  as  the  loading  increases.  This  is  most 
probably  because  the  lower  speeds  at  the  higher  loads  allow  more 
accurate  reading  of  the  motion  picture  film.  Use  of  an 
accelerometer  would  seem  to  be  the  preferred  method  since  the 
accuracies  are  comparable  and  the  accelerometer  output  does  not 
require  the  tedious,  manual  interpretation  that  is  required  of  the 
motion  film  data. 


TABLE  4 . 1 


PERCENT  DIFFERENCE  BETWEEN  EXPERIMENTAL  TECHNIQUES 

LOAD 

X 

Y 

0  Kg 

1.2 

15 

2.115  Kg 

0 . 50 

9.3 

4.233  Kg 

0.44 

6.2 
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EXPERIMENTAL  X  TIP  POSITION  (2.1  15  Kg) 
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Figure  4.3.  Comparison  Between  Experimental  Techniques. 
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B.  TIP  POSITION 


After  evaluating  the  different  techniques  for  determining  arm 
tip  position  the  next  step  in  the  validation  of  the  ERLS  dynamic 
model  was  to  compare  the  actual  arm  tip  position  as  determined 
by  accelerometer  with  the  arm  tip  position  predicted  by  the  two 
different  simulation  models. 

Evaluation  of  the  data  centered  around  the  parameters 
amplitude  and  frequency.  These  were  evaluated  in  Petroka's  thesis 
[Ref.  12]  and  for  a  consistent  approach  in  validating  the  ERLS 
model  are  evaluated  in  this  thesis.  All  criteria  for  comparison 
were  also  adopted  from  Petroka’s  thesis.  These  include  a  relative 
percentage  error  of  +/-  10%  as  the  standard  for  comparison.  The 
tip  position  amplitude  error  is  computed  by  taking  the  square  root 
of  the  sum  of  the  squares  of  the  differences  between  the 
theoretical  and  experimental  X  and  Y  coordinate  positions.  The 
first  mode  tip  frequency  errors  are  computed  by  taking  the 
difference  between  the  theoretical  and  experimental  tip  position 
frequencies.  Only  the  first  mode  frequency  is  determined  since  it 
was  the  dominant  mode.  Errors  were  then  normalised  to  obtain 
relative  errors.  Tip  position  amplitude  was  normalised  using  arm 
length.,  and  frequencies  were  normalised  using  experimental 
frequencies. 
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Figures  4.7-4.12  represent  the  graphical  results  of  the 
comparison  of  experimental  arm  tip  position  with  theoretical  arm 
tip  position.  Results  are  given  in  terms  of  the  global  coordinate 
system.  The  loading  conditions  were  no  load,  a  2.115  kg  load  and 
a  4.233  kg  load.  The  initial  condition  for  all  runs  was  the 
horizontal  position  of  the  flexible  arm.  Servovalve  excitation  for 
all  runs  was  a  step  input  of  4.0  milliamps.  The  initial  tip 
deformation  and  slope  as  determined  by  elementary  beam  theory 
were  initial  conditions  input  into  the  simulation  program. 

The  figures  and  results  tabulated  in  Tables  4.2  and  4.3 
indicate  that  both  models  satisfy  the  evaluation  criteria  of  10  % 
error.  The  modal  shape  function  gives  us  slightly  better  results 
percentage  wise  with  regards  to  tip  position  amplitude,  but  when 
converted  to  actual  distances  these  small  percentage  differences  can 
mean  significant  improvements  in  tip  position  accuracy  since  a  1  % 
improvement  is  on  the  order  of  a  1  centimeter  improvement  in 
accuracy.  The  acceptable  tip  position  amplitude  error  occurs 
despite  the  fact  that  we  are  using  a  truncated  model  of  only  the 
first  two  modes. 

The  differences  in  amplitude  observed  in  the  arm  tip  position 
amplitude  can  attributed  to  several  causes.  Experimentally  there 
are  errors  associated  with  recording  the  data  such  as  calibration 
errors  and  conversion  errors.  The  cubic  shape  function  is  limited 
by  the  single  element  modeling  which  will  result  in  a  more  rigid 
model.  The  modal  shape  function  model  suffers  from  the 
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TABLE  4.2 


RELATIVE  ERRORS  OF  TIP  POSITION  AMPLITUDE 

LOAD 

CUBIC 

MODAL 

0  Kg 

5.3  % 

4.9  % 

2.115  Kg 

7.9  % 

6.3  % 

4.233  Kg 

8.3  % 

6.3  % 

TABLE  4.3 


RELATIVE  ERROR  OF  FREQUENCY 

LOAD 

CUBIC 

MODAL 

0  Kg 

<  5  % 

<  5  % 

2.115  Kg 

<  5  % 

<  5  % 

4.233  Kg 

<  5  % 

<  5  % 
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truncation  effects  since  the  flexible  arm  is  theoretically  composed  of 
and  infinite  number  of  modes  and  we  have  only  include  the  first 
two  modes.  Neglecting  axial  deformation  and  assuming  small 
bending  displacements  also  effects  the  accuracy  of  our  model. 
These  effects  are  most  noticeable  during  the  first  few  tenths  of  a 
second  of  motion.  During  this  time  arm  tip  position  decreases  but 
an  increase  in  predicted  by  both  the  cubic  and  modal  shape 
functions.  The  theoretical  arm  position  approximates  the  actual 
arm  position  but  since  the  the  small  motion  displacement  is 
measured  with  respect  to  the  ERLS  local  coordinate  system  the 
theoretical  arm  length  appears  to  increase  particularly  under 
heavy  loading. 

The  frequency  results  were  similarly  promising.  All  met  the 
error  criteria  of  10  %.  Only  the  fundamental  frequencies  were 
observed  indicating  the  dominance  of  the  first  mode. 


C.  STRAIN 

The  final  step  in  the  experimental  validation  of  the  ERLS  model 
of  a  flexible  manipulator  was  to  compare  experimental  strain  with 
the  strain  predicted  from  the  two  simulation  models. 

Amplitude  and  frequency  were  evaluated  as  in  the  tip  position 
experiments.  In  addition,  the  error  criteria  of  10  %  was  also 
maintained.  The  strain  amplitude  and  frequency  errors  were 
obtained  by  taking  the  difference  between  the  theoretical  and 
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experimental  strain  data.  The  results  are  normalised  using  the 
respective  experimental  values.  The  strain  amplitude  is  actually  a 
superposition  of  the  first  two  modes  but  the  fundamental  mode  is 
dominant.  The  strain  frequency  is  easily  separated  into 

fundamental  and  secondary  modes. 

Figures  4.13-4.15  are  the  graphical  results  of  the  comparison 
between  experimental  strain  and  theoretical  strain  as  predicted  by 
the  ERLS  using  a  a  modal  shape  function.  The  loading  conditions 
were  no  load,  a  2.115  kg  load  and  a  4.233  kg  load.  The  initial 
condition  for  all  runs  was  the  vertical  position  of  the  flexible  arm. 
Servovalve  excitation  for  all  runs  was  a  step  input  of  4.0 
milliarnps.  These  results  are  summarised  in  Tables  4.4  and  4.5. 

The  strain  amplitude  values  exeed  the  10  %  error  criteria. 
The  amplitude  of  the  theoretical  strain  is  usually  less  than  the 
experimental  values.  This  is  to  be  expected  from  the  truncation  of 
the  modal  shape  function .  The  addition  of  more  modes  to  the 
model  should  bring  the  amplitude  closer  to  the  experimental 
values.  The  values  predicted  by  the  modal  shape  function  are 
much  closer  than  the  cubic  shape  function  results  predicted  in 
Petroka's  thesis  [Ref.  12], where  errors  were  on  the  order  of 
35-40  %. 

Perhaps  the  most  significant  result  in  the  choice  of  a  modal 
shape  function  over  a  cubic  shape  function  is  in  the  area  of  strain 
frequency.  Fundamental  frequencies  are  accurately  modeled  by 
both  models  with  all  results  less  than  5  %.  However  the  modeling 
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of  the  secondary  mode  frequency  is  much  more  accurately  made 
with  the  modal  shape  function.  The  cubic  shape  function  results 
in  errors  on  the  order  of  20-40  %  [Ref  12].  The  modal  shape 
function  improves  on  these  to  the  point  where  all  errors  satisfy  the 
10  %  criteria.  The  theoretical  values  are  all  slightly  greater  than 
the  experimental  values,  indicative  of  the  more  rigid  model  that 
results  from  the  truncation  of  the  modal  shape  function. 
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STRAIN  VS.  TIME  (4.233  Kg) 
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Figure 


TABLE  4.4 


RELATIVE  ERROR  OF  STRAIN  AMPLITUDE 

LOAD 

CUBIC 

MODAL 

0  Kg 

38  % 

17.9  % 

2.115  Kg 

38  % 

17.4  % 

4.233  Kg 

41  % 

23.7  % 

TABLE  4.5 


RELATIVE  ERROR  OF  FREQUENCY 

LOAD 

FUNDAMENTAL 

SECOND  MODE 

CUBIC  MODAL 

CUBIC 

MODAL 

0  Kg 

07 

D  ,'o 

CO 

to 

7.5  % 

2.115  Kg 

<  5  % 

26  % 

<  5  % 

4.233  Kg 

<  5  % 

22  % 

5.5  % 
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SUMMARY 


V. 


A.  CONCLUSIONS 

The  purpose  of  this  thesis  is  to  experimentally  validate  the 
ERLS  dynamic  model  of  a  flexible  manipulator.  This  validation  is 
an  essential  part  of  the  long-term  goal  of  designing  and  controlling 
a  flexible  manipulator.  The  model  chosen  was  Chang’s  [Ref.  5] 
Equivalent  Rigid  Link  System  dynamic  model.  The  model  was 
adapted  to  our  experimental  manipulator,  a  single- link, 
hydraulically  actuated  flexible  arm  moving  in  the  vertical  plane. 
The  thesis  investigated  experimental  techniques  and  a  comparison 
of  tip  position  amplitude  and  strain  for  the  two  simulation  models. 

The  results  indicate  that  an  accelerometer  and  motion  picture 
studies  of  arm  tip  motion  yield  comparable  results.  The 

accelerometer  has  the  advantage  in  that  it  provides  a  continuous 
signal  and  does  not  require  the  tedious  manual  interpretation  that 
is  required  of  the  motion  picture  data.  This  means  an  appreciable 
savings  of  time. 

The  tip  position  amplitude  comparison  between  the 
experimental  results  and  the  results  obtained  from  the  two  choices 
of  shape  function  indicated  that  the  choice  of  natural  modes  yields 
a  slight  percentage  improvement  in  tip  position  accuracy,  but  this 
improvement  can  be  significant  in  terms  of  distance  particularly 
with  regard  to  future  control  designs.  Both  shape  functions 
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accurately  modeled  the  dominant  first  mode  frequency  of  the  arm 
tip  position. 

The  comparison  between  experimental  strain  and  the  two 
theoretical  simulations  demonstrated  that  the  natural  mode  shape 
function  leads  to  significant,  improvements  in  both  strain  amplitude 
and  frequency . 

The  limitations  of  both  models  are  a  direct  .  result  of  some 
modeling  assumptions  and  choices.  The  natural  mode  shape 
function  is  only  composed  of  the  first  two  modes.  This  truncation 
accounts  for  the  lower  values  for  both  tip  position  and  strain 
amplitude.  This  also  yields  a  more  rigid  model,  thus  the  slightly 
higher  frequencies  for  the  modal  shape  function.  Similarly,  the 
cubic  shape  function  is  restricted  by  its  single  element  modeling. 
This  more  rigid  representation  accounts  for  the  cubic  shape 
functions  lower  amplitudes  and  higher  freqeuncies. 

The  small  displacement  assumption  leads  to  noticeable 
differences  in  tip  position  amplitude  particularly  during  the  first- 
few  tenths  of  a  second  of  motion  in  the  X  direction.  No  effects  of 
the  small  displacement  assurntion  are  noted  in  the  strain  results. 

The  experimental  and  simulation  results  provided  a  valuable 
validation  of  the  ERLS  model.  The  choices  of  a  natural  mode 
shape  function  and  an  accelerometer  for  tip  position  determination 
offer  improvements  to  the  model  and  further  suggest  the  potential 
benefits  of  the  ERLS  as  a  model  for  flexible  manipulator. 
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B .  RECOMMEND  ATI  ON3 


The  long-term  goal  of  this  research  is  the  design  and  control  of 
a  flexible  manipulator  system.  Simulation  studies  need  to  continue 
on  the  validation  of  the  ERLS  model.  Shear  deformation  and 
rotary  inertia  effects  should  be  considered  in  future  work.  In 
addition,  the  effects  of  truncation  also  need  to  be  studied.  The 
significance  of  each  of  the  terms  in  the  equations  of  motion  would 
be  helpful  in  improving  the  model. 

Control  system  design  requires  significant  investigation. 
Specific  areas  of  importance  include  control  schemes  to  achieve 
motion  control  and  force  control.  Continued  studies  must  be  made 
in  the  area  of  finding  sensors  that  will  provide  accurate  and  fast 
feedback  signals.  Hardware  implementations  to  make  eventual  on¬ 
line  control  possible  should  also  be  investigated. 


APPENDIX 


A 


DERIVATION  OF  THE  EQUATIONS  OF  MOTION  FOR  THE 
EXPERIMENTAL..  SINGLE-LINK.,  FLEXIBLE  MANIPULATOR 

A  principal  idea  of  the  ERLS  is  to  separate  the  motion  of  a 
flexible  manipulator  system  into  a  large  motion  and  a  small 
motion.  The  large  motion  represents  the  the  global  motion  of  the 
system  described  by  the  ERLS  and  the  small  motion  displacements 
are  described  with  respect  to  the  ERLS.  The  Lagrangian  dynamics 
approach  is  used  for  the  derivation  of  the  equations  of  motion  due 
to  its  straightforward  and  systematic  nature  which  is  helpful  in 
the  analysis  of  complex  systems.  This  formulation  requires 
generalised  coordinates.  Since  the  global  motions  are  separated 
into  large  motions  and  small  motions,  generalised  coordinates  are 
defined  to  represent  these  two  motions.  The  three  generalised 
coordinates  chosen  are  the  rigid  body  rotation,  G,  and  two  nodal 
displacements,  v(0)  and  <$>(G),  which  can  be  measured  at  the  end  of 
the  link.  The  following  are  the  two  sets  of  Lagrange,  equations 
used  to  develop  the  equations  of  motion: 

d/dt  (0KE/38)  -  9KE/98  +  3 PE/ 9 8  =  F 
d/dt  (9KE/9U)  -  3KE/3U  +  3PE/3U  =  0 


KE  -  kinetic  energy 
PE  -  potential  energy 


0 


-  large  motion  joint  variable,  theta,  measured  between 

the  ERLS  link  and  the  global  X  axis 
-2x1  vector  of  small  motion  displacement  and  slope, 
v(0)  and  0(0) 

F  -  generalised  force  for  large  motion,  applied  moment 

Kinetic  energy  of  the  system  is  due  to  kinetic  energy  of  the 
link,  kinetic  energy  of  the  actuator  and  the  kinetic  energy  of  any 
load .  The  actuator  is  treated  as  a  rigid  body  and  all  kinetic 
energy  terms  are  calculated  separately.  Generalised  forces 

include  any  applied  forces  and  damping  forces.  The  expressions 
used  for  the  determination  of  kinetic  energy  are  as  follows: 


KE  (arm)  -  l/2  J  Ji  RT  (R)  dv 

RftM 

VOLUME 


KE  (load)  -  1/2  Trace  J  pL  RLT  (RJ  dv 

LORD 

VOLUME 


KE  (rotor)  —  1/2  Trace  J  jip  Rp^  (Flp)  dv 

ROTOR 

VOLUME 


The  global  position  vector  of  the  arm  is  determined  form  the 


following  transformation : 


64 


R  =  UJ  (r  +  d) 

UJ  =  the  3  x  3  transformation  matrix  ,  function  of  theta 
only 

r  =  the  3x1  local  position  vector  of  the  arm  measured 

from  the  coordinate  system  whose  origin  is  at  the  end  of 
the  ERLS  link 

d  =  the  3  x  1  deformation  vector  that  only  includes  the 

transverse  displacement,  v.  Derivation  of  this  vector  is 
made  in  Chapter  2. 

jj.  =  the  mass  density  of  the  steel  arm 

The  global  position  vector  of  the  load  is  determined  from  the 
following  transformation : 


Dl  =  the  3x3  transformation  matrix  due  to  the  local 

deformations  of  the  arm  tip 
rL  =  the  3  x  1  local  position  vector  for  the  load 
=  mass  density  of  the  load,  steel 
The  global  position  vector  of  the  hydraulic  actuator  is  given  by 
the  following  transformation: 


Rr  ”  «fi  rR 
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Af.  =  the  3x3  transformation  matrix  due  to  the  large  motion 
rotation  of  the  rotor 

i~pj  =  the  3  x  1  local  position  vector  for  the  rotor 
Up.  =  the  mass  density  of  the  actuator  rotor,  aluminum 
Potential  energy  of  the  system  comes  from  strain  energy  due 
to  deformation .  and  from  gravity .  The  expressions  used  for  the 
determination  of  potential  energy  are  as  follows: 


PE 


d 


1/2  J  E  1  „ 

LINK 

LENGTH 


/  "\r> 
IV  / 


dx 


PEg  =  -  J  JJ.  rT  g  dv 

LINK 

VOLUME 


=  -Jr  »-lt  §  dv 

LOAD 

VOLUME 


E  I22  =  the  flexural  rigidity  in  the  2  direction,  perpendicular  to 
the  plane  of  motion 

v"  =  second  derivative  of  the  transverse  displacement  v 

with  respect  to  the  the  local  x  coordinate  direction 
expressed  in  terms  of  v(0)  and  4>(0). 

g  =  gravitational  acceleration  vector 
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To  simplify  the  expressions  in  the  equations  of  motion  the 
following  definitions  for  inertia  terms,  adapted  from  the  expressions 
found  in  Reference  5,  are  used: 


~  J^l  *l*l  L*v 

LOAD 

VOLUME 


=  the  3  x  3  inertia  matrix  of  the  load 


>R  -  K  rBrRT  dU 

ROTOR 

VOLUME 


=  the  3  x  3  inertia  matrix  of  the  rotor 


1 1 1 1  (llleT  ,  IU8)  =  J  J.J.  rT  lUeT  LUe  r  dv 

LIHK 

VOLUME 


1112  (IU0T  ,  HI)  =  f]J.  rT  III  0T  LU  $  dv 

LINK 

VOLUME 


1121  (ill T  ,  UJe)  =  I  J.J.  (J)T  lUeT  III  r  dv 

LINK 

VOLUME 


1122  (lllT  ,  lU)  =  j  jj.  <J>T  1111  III  dv 

LINK 

VOLUME 


6? 


1122 (IJJT  ,  LUr)  =  Jjj.  4>t  UJt  lilft  <J)  dv 

LINK 

VOLUME 


1122 (llleT  ,  ULl)  =  jp  4>T  UUT  UU  $  dv 

LINK 

VOLUME 


I122«  ,  UJ#)  -  Jp  <J.T  W9T  UOe  <J>  dv 

LINK 

VOLUME 


Ivy  =  J  Ml  V2  dv 

LORO 

VOLUME 


W. 


Jr  x2  dv 

LORD 

VOLUME 


=  the  3  x  2  link  shape  matrix  defined  in  Chapter  2 

lilfi  =  derivative  of  the  3x3  matrix  IU  with  respect  to  the 

joint  variable  theta 

UUr  =  result  of  the  simplification  of  the  second  time 

derivative,  of  the  transformation  matrix  III  and  is 
termed  the  residual  acceleration. 
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The  following-  definitions  are  utilised  to 


simplify  the 


computations  and  the  expressions  used  for  computer  coding  of  the 
equations  of  motion: 


k„  -  f  rT  c  r  ci>: 

LINK 

LENGTH 


H„  =  J  M  rT  civ 

LINK 

VOLUME 


H,,  =  J  p.L  <tT  dv 

LINK 

VOLUME 


H41  =  J'  J4  rL T 

LINK 

VOLUME 


T  =  the  second  derivative  of  the  shape  function  matrix 

C  =  the  3  x  3  flexural  rigidity  matrix  including  only 

E  I  22 

The  actual  derivation  of  the  equations  of  motion  requires 
detailed,  tedious  substitution  of  the  expressions  for  kinetic  energy 
and  potential  energy  into  the  Lagrange  equations,  and  the 
Lagrangian  formulation  yields  two  sets  of  equations.  One  set 
describes  the  large  motions  and  the  other  set  describes  the  small 
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motions.  ■  These  two  sets  of  equations  are  non-linear,  coupled, 
second-order,  ordinary  differential  equations  represented  as  follows. 


8  +  M  U  =  F 
qq  q 


(2-1) 


M  8  +  M  Li  +  K  Ll  =  F 

riq  r,ri  q  n 


where 

M 


qq 


M 


qq 


M 


qq 


M 


qq 

\ 

q 


=  1  x  1  effective  inertia  matrix  for  large  motions 
=  1  x  2  coupled  inertia  matrix  of  the  small  motion  effect 

on  large  motion 

=  2x1  coupled  inertia  matrix  of  the  large  motion  effect 
on  small  motions 

=  2  x  ?  effective  inertia  matrix  for  small  motions 


=  2x2 


=  2  x 


8 


=  2 


stiffness  matrix  for  small  motions 
load  vector  for  the  large  motions 
load  vector  for  the  small  motion 

second  derivative  of  the  generalised  coordinate  of 


the  large  motions 


The 


vector  generalised  coordinate  of  the 

deformations  representing  the  small  motions 
coefficients  for  the  terms  are  defined  as  follows: 


M  =  1 1 1 1  (LL10T  ,  IU9)  +  UT  1122  (LU0t  ,  W0)  U  + 
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Trace  (  tU0  DL  IL  Dj  IU0T  +  AD  \B  Aj  ) 


*r  R  R 


Mqr)  =  1112 (l.U0T  ,  III)  +  ((Ml  L  +  Mx),  (M;.  L  +  Ixx +!,,,,) 


L  is  the  arm  length.  M,  is  the  mass  of  the  load.  M..  is  the 

°  L  a 

first  moment  of  the  load  with  respect  to  the  local  y  axis. 

F,  -  2.  UT  1122  (life1  ,  til)  U  HnlU,Tg  +  UT  Hj,  UU9Tg  - 

Trace (Ufe  BL  IL  Dj  UJr  +  2.  UJg  DL  1L  Dj  W  +  Am  IrArr) 

+  H„  Dj  UfeT  g  +  T 


T  is  the  externally  applied  torque.  Af;R  is  a  result  of  a 

simplification  of  the  second  time  derivative  of  the  transformation 
matrix  Af>  and  is  termed  DLp  residual  acceleration.  Ac>a  is  a 

derivative  of  with  respect  to  the  joint  variable  theta. 

-  (  Trace  (  IU8  0L  IL  DL1|T  IUT),  Trace(llfe  DL  IL  BU,T  IUT)) 

+  1121  (lllT  ,  Ufe) 

BU1  and  E'L)2  are  the  derivatives  of  the  arm  tip  deformation 

transformation  matrix  differentiated  with  respect  to  nodal 
deflection  displacement  and  the  nodal  slope  displacement 
respectively . 

=  1122  (ill1  ,  III)  +Ml  (  V(0l  )  +  (  IXX  +  IYV)  x  (  $10)  ) 
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K  = 
r\ 

K„  +  1 122  (LUt  ,  Ui„) 

F 

n 

H,,WTg  -  (Trace (liiR  DL  IL  DU|T  LUT  +  2 . LU  DL IL  DL],T  IUT), 
Trace (LUr  Dl  1l  Dlot  IUT  +  2. 01  0L IL  DL,,T  UJT))  + 

h4,  Du;  UJt  g.  h4,  dl12t  IUT  g 

These 

expressions  are  coded  and  solved  for  in  the  computer 

program  listed  in  Appendix  B 
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APPENDIX  B 


LISTING  OF  THE  DSL  CODING  UTILIZED  IN  SOLVING 
THE  DYNAMIC  EQUATIONS  OF  MOTION 


*  THESIS  COPV 

* 

*  SIMULATION  OF  SINGLE  LINK  FLEXIBLE  MANIPULATOR  DYNAMICS 

*  - 

+ 

* 

*  THIS  PROGRAM  SOLUES  THE  ERLS  FLEXIBLE  MANIPULATOR  DYNAMICS  FOR  A 

*  SINGLE  LINK  EXPERIMENTAL  ARM.  THE  EXPERIMENTAL  ARM  PARAMETERS  ARE 

*  INPUTTED  AND  THE  HYORRUL I C  ACTUATION  DYNAMICS  ARE  INCLUDED  IN  THE 

*  SIMULATION.  THE  INPUT  IS  THE  CURRENT  TO  THE  SEFlUOUALUE  MOUNTED  ON 

*  THE  HYDRAULIC  ACTUATOR  AND  THE  OUTPUT  IS  THE  POSITION  OF  THE  ARM 

*  TIP  IN  THE  GLOBAL  REFERENCE  SYSTEM.  THE  COOING  CONSISTS  OF  A  MAIN 

*  PROGRAM,  TEN  FUNCTION  SUB-PROGRAMS  AND  EIGHTEEN  SUBROUTINES. 

* 

*  THE  FOLLOWING  PARAMETERS  ARE  DEFINED: 

*  1 . fl-EFFECT I UE  CROSS-SECTIONAL  AREA  OF  FLEXIBLE  ARM 

*  2 . RRRDD-3X3  SECOND  TIME  DEAIURTIUE  OF  ROTOR  RESIDUAL  ACCELERATION 

*  MATRIX 

*  3 . ARTH-3X3  ROTOR  TRANSFORMATION  MATRIX  DIFFERENTIATED  WITH  RESPECT 

*  TO  THETA 

*  4. A  IP-SHAPE  FUNCTION  MATRIX  COEFFICIENT  DEFINED  IN  APPENDIX  B 

*  5 . A2P-SHAFE  FUNCTION  MATRIX  COEFFICIENT  DEFINED  IN  APPENDIX  B 

*  6 . BE-EFFECT I UE  BULK  MODULUS  OF  FLUID 

*  ?. BETA  1 -SHAPE  FUNCTION  MATRIX  COEFFICIENT  DEFINED  IN  APPENDIX  B 

*  S.BETR2-SHAPE  FUNCTION  MATRIX  COEFFICIENT  DEFINED  IN  APPENDIX  B 

*  9.BIGF-3X1  RIGHT-HAND  SIDE  UECTQR  FOR  LARGE  AND  SMALL  MOTION 

*  ACCELERATIONS 

*  10 . B I GM-3X3  MATRIX  OF  LARGE  AND  SMALL  MOTION  ACCELERATION 

*  COEFFICIENTS 

*  1 1 . CTM-TOTAL  LEAKAGE  COEFFICIENT  OF  THE  ACTUATOR 

*  12 . C 1 , C2, C3, C4 -SHAPE  FUNCTION  MATRIX  COEFFICIENTS  DEFINED  IN  APPENDIX  B 

*  16 . DEFM-D I SPLACEMENT  DEFORMATION  MAR  I  ABLE 

*  17.DEFMD-T I  ME  DERIVATIVE  OF  DISPLACEMENT  DEFORMATION  VARIABLE 

*  1 3 . D I FF , QERR , QERR 1 . FACTOR-DUMMY  UAR I ABLES 

*  19. OL 1-3X3  DEFORMATION  MATRIX 

*  20. DL 11-3X3  DEFORMATION  MATRIX  DIFFERENTIATED  WITH  RESPECT  TO  THE 

*  DISPLACEMENT  DEFORMATION  VARIABLE 

*  2 1.DL 12-3X3  DEFORMATION  MATRIX  DIFFERENTIATED  WITH  RESPECT  TO  THE 

*  SLOPE  DEFORMATION  VARIABLE 

*  22. DL  ID- 3X3  FIRST  TIME  DERIVATIVE  OF  DEFORMATION  MATRIX 

*  23 . DM-RCTURTOR  DISPLACEMENT 

*  24. E-MODULUS  OF  ELASTICITY  OF  STEEL 

*  25.FN-2X1  RIGHT-HAND  SIDE  VECTOR  FOR  SMALL  MOTION  ACCELERATIONS 

*  26 . FQ-R I GHT-HAND  SIDE  FOR  LARGE  MOTION  ACCELERATIONS 

*  27.G-3X1  GRAVITATIONAL  ACCELERATION  VECTOR 

*  2S.GP0S-3X1  GLOBAL  POSITION  VECTOR  FOR  ARM  TIP 

*  29 .  H 1 1  - 1 X3  LINK  FI  FiST  MOMENT  OF  I NERT I A  VECTOR 
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*  30. H2 1-2X3  LINK  SHAPE  MATRIX  FIRST  MOMENT  OF  INERT IAUECTOR 

*  31.H41-1X3  LOAD  FIRST  MOMENT  OF  INERTIA  UECTOR 

*  32.KCE-T0TAL  FLOW  PRESSURE  COEFFICIENT 

*  33 . PL -LOAD  HYDRAULIC  PRESSURE  DROP 

*  34. PS-HYDRAULIC  SUPPLY  PRESSURE 

*  35 . QL-FLOW  DELIVERED  FROM  THE  SERVOVALVE 

*  36. SLOP-SLOPE  DEFORMATION  VARIABLE 

*  37 . SLOP'D- T I  ME  DERIVATIVE  OF  SLOPE  DEFORMATION  VARIABLE 

*  38.S0L-3X1  VECTOR  OF  LARGE  AND  SMALL  MOTION  ACCELERATIONS 

*  39.TE-T0RQUE  EFFICIENCY 

*  40 . TH-LRROE  MOTION  POSITION  VARIABLE 

*  4 1 . THD-T I  ME  DERIVATIVE  OF  LARGE  MOTION  VARIABLE 

*  42. TORQUE-APPLIED  TORQUE  BY  ACTUATOR 

*  43.U-2X1  ARM  TIP  DEFORMATION  VECTOR  INCLUDING  DISPLACEMENT  AND  SLOPE 

*  44.UQ-2X1  ARM  TIP  DEFORMATION  VECTOR  DIFFERENTIATED  WITH  RESPECT  TO 

*  TIME 

*  45. VT-TQTRL  COMPRESSED  VOLUME  INCLUDING  ACTUATOR  LINES  AND  CHAMBERS 

*  •  46.W-3X3  LINK  TRANSFORMATION  MATRIX 

*  47. HD- 3X3  FIRST  TIME  DERIVATIVE  OF  LINK  TRANSFORMATION  MATRIX 

*  48 . HRDD-3X3  SECOND  TIME  DERIVATIVE  OF  LINK  RESIDUAL  ACCELERATION 

*  MATRIX 

*  49 . MTH-3X3  TRANSFORMATION  MATRIX  DIFFERENTIATED  WITH  RESPECT  TO 

*  THETA 

*  50 . X I FRAC-VAR I  ABLE  FRACTIONAL  AMOUNT  OF  INPUT  CURRENT  TO  SERVO- 

*  VALVE 

*  51.X I  IMP-CURRENT  INPUT  EQUAL  TO  INITIAL  AND  FRACTIONAL  AMOUNTS 

*  52 . X I L-3X3  INERTIA  MATRIX  OF  THE  LOAD 

*  53. X 10- INITIAL  INPUT  CURRENT  TO  SERVOVALVE 

*  54 . X I R-3X3  ROTOR  INERTIA  MATRIX 

*  55. XI STEP-STEP  INPUT  OF  FRACTIONAL  AMOUNT  OF  INPUT  CURRENT 

*  56 . XK 1 1-2X2  PART  I AL  L I NK  ST  I FFNESS  MATR I X 

+  57 . XKN-2X2  LINK  STIFFNESS  MATRIX 

*  58 . XKV- SERVOVALVE  SIZING  CONSTANT 

*  59 . XLL-LENGTH  OF  FLEXIBLE  ARM 

*  60. XML-MASS  OF  LOAD 

*  6 1 . XMMH-2X2  COEFFICIENT  MATRIX  OF  SMALL  MOTION  ACCELERATIONS  IN  THE 

*  SMALL  MOTION  DYNAMIC  EQUATIONS 

*  62 . XNNQ-2X 1  COEFFICIENT  VECTOR  OF  LARGE  MOTION  ACCELERATIONS  IN  THE 

*  SMALL  MOTION  DYNAMIC  EQUATIONS 

*  63.XMQM- 1X2  COEFFICIENT  VECTOR  OF  SMALL  MOTION  ACCELERATIONS  IN  THE 

*  LARGE  MOTION  DYNAMICS  EQUATION 

*  64 . XMQQ-COEFF I C I  ENT  OF  LARGE  MOTION  ACCELERATION  IN  THE  LARGE  MOTION 

*  DYNAMICS  EQUATION 

*  65 . XMQQP-2X2  DUMMY  MATRIX  FOR  USE  IN  FORMULATING  THE  EQUATIONS  OF 

*  NOT  I  ON 

+  66 . XMR-MASS  OF  ACTUATOR  ROTOR 

*  67 . XMU-MASS  DENSITY  OF  STEEL  FLEXIBLE  ARM 

*  68 . XI1X-F I RST  MOMENT  OF  LOAD  HITH  RESPECT  TO  THE  LOCAL  COORDINATE 

*  Y  AXIS 

*  69. XX I -VARIABLE  REPRESENTING  INERTIA-LIKE  LOAD  PROPERTY 

*  70 . V-DUMMV  VARIABLE  TO  STORE  INTEGRATION  RESULTS 

*  71 .VY I -VARIABLE  REPRESENTING  INERT  I A-L IKE  LORD  PROPERTY 

*  72. 2 1 -AREA  MOMENT  OF  INERTIA  OF  FLEXIBLE  ARM 

+• 

INITIAL 
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* 

* 

* 


INITIAL  UftLUES  OF  PARAMETERS  ARE  IHPUTTED  01 A  XINIT  SUBROUTINE 


D  DIMENSION  U(2, 1 >,XI1C!Q( 1 >,XN0.qP(2,2),DL1<3,3),WTH(3,3>,fiRTH(3,3>, 

D  #XIR(3,3>,  XMQNC  1 , 2 ) .  UOC  2 ,  1  >,  H  1 1  ( 1 . 3  > .  GC3 ,  1  >,  H2  1(2 , 3 >, 

D  *WRDD(3 , 3  >,  DL  10(3, 3  >, WD(3, 3 ),  ARRDD(3, 3  >,  H4 1  <  1 , 3  ),  XK  11(2,2), 

D  #DL 12(3,3), XMMQ <  2 , 1 ) , U  < 3 , 3 > , XMNN ( 2 , 2 > , XKN ( 2 , 2  > , FM ( 2 , 1 ) , B I  CM < 3 , 3 ) , 

D  #BIGF<3. 1 ),XIL<3, 3 ),DL 1 1(3,3),DEFMD( 1 ),S0L(3), THO( 1 ),SLOP( 1), 

D  #SLGPD<  1 ) , A< 1 ) , E(  1 ) , 2 1 ( 1 ) , XX I ( 1 ), VV I ( 1 ) , FQ( 1 ) , GF0S(3 ), X I TH( 1 ) , 

D  «XMU(  1 ), XLL(  1  XML (  1  >,  XMR<  1  >,XMX<  1 ) .  THC 'l ) .  TORQUE <  1  >,DEFM<  1  >, 

D  *PS(  1  >,  X I  FF;AC(  1  > ,  X 1 0  ( 1 ),  CTM<  1  >,  OT  ( 1 ),  BE<  1  >,  DM<  1 ),  XKO(  1  > ,  TE<  1  >, 

D  #QL(  1 ),  PL(  1 ),  D I  FF(  1 ),  X I  I  NP(  1 ),  QEP.R  1  ( 1 ),  QERR(  1),  FACTOR  ( 1 ),  X I  STEP( 1  ) 


FIXED  I 
NOSURT 
* 

*  COMMON  BLOCK  TO  PASS  CONSTANTS  USED  IN  THE  SHAPE  FUNCTION  MATRIX 

*  DERI  OAT  I  ON  OF  THE  CONSTANTS  INCLUDED  IN  APPENDIX  B. 

* 

D  COMMON /FCDRT A /C 1 , C2, C3, C4, A  IP, R2P, BETA  1 , EETR2 

* 

C  1=0. 5 15462 194 
C2=0 . 149362234 
C3=-0 . 107657991 
C4=-0. 205906794 
A 1P= 1.362220557 
R2P=0. 98 1867539 
BETA  1=1. 877920950 
BETA2=4. 70 1142347 

+ 

*  INITIALIZATION  SUBROUTINE 

* 

CALL  X I N I T ( TH . THD , DEFM, DEFMD , SLOP , SLOPD , 00 . POSO , A , XML . XMU , . . . 

XLL ,  XMR ,  E, Z I ,  PS,  XI FRRC ,  X 1 0,  Cf II .  OT ,  BE .  DM,  XKU ,  TE,  Ql _ 

PL.PLIC) 

+ 

DERI  OAT  I OE 
* 

*  COEFFICIENTS  FOR  BOTH  LARGE  AND  SMALL  MOTION  ACCELERATIONS 

*  AND  THE  RIGHT-HAND  SIDES  ARE  COMPUTED  IN  THE  FOLLOWING 

*  SUBROUTINE  CALLS.  ALSO, THE  HYDRAULIC  DYNAMICS  ARE  INCLUDED 

*  IN  THE  MAIN  PROGRAM. 


NOSORT 

* 

+  HYDRAULIC  DYNAMICS  (  MERRITT,  CHRPTER  SIX  ) 

*  CURRENT  TO  ACTUATOR  IS  CHANGED  BY  CHANGING  IFRAC  IN  XINIT 

* 

X I STEP( 1 >=X I FRAC( 1 )+STEP(0 . 0 ) 

XI INP< 1 >=X!0( 1 >+X!STEP(  1 ) 

IF(PL< 1>.GT.PS<  1  )>  GO  TO  2 
GO  TO  3 

2  PL( 1 )=PS( 1 ) 

3  QERR 1 < 1 )=  < X 1 1  HP ( 1 )*XKU ( 1 )+DSQRT ( PS ( 1 >-PL ( 1 > > >- ( DM ( 1 >*THD( 1 >> 
QEFlR<  1  )=QERFi  1  (  1  )/CTM(  1 ) 
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0 1  FF(  1  >=QERR<  1  )-F'L(  1 ) 

FACTOR ( 1  )=UT ( 1 >/(4 . ODO*BE< 1 >+CTM< 1 > ) 
DIFFK  1  )=DIFF<  1 ) 


SORT 

PL  1=1 NTGRLCPLI C,D I FF 1 ,  1) 

HOSORT 

PL< 1 >=PL 1 ( 1 ) /FACTOR < 1 ) 

TORQUE < 1 )=TE< 1 >*PL< 1 >*OH< 1 > 

+ 

*  MATRIX  AHO  UECTOR  FOFiMULAT  I  OH  SUBROUT  I  HE 

* 

CALL  FORM ( M , NTH , NO , DL 1 , DL 1 D , X I L , X I R , ARTH ,  URDU ,  RRFlOO ,  U ,  UD ,  .  .  . 

XMQQP, G , H 1 1 , H2 1 . DL  1 1 , DL  12, H4 1 , XK 11 , A , XMU, XML , XLL , TH , THD, . . . 

DEFM, DEFMD , SLOP , SLOPD ,  E, Z I , X11R,XMX, VV I , XX I ) 

* 

*  COEFFICIENT  OF  LARGE  MOTION  ACCELERATION  IN  LARGE  MOTION  DYNAMICS 

*  EQUATION  SUBROUTINE 

* 

CALL  XLMHQQCXMQQ, U, XMQQP, DL  1 , NTH, ARTH, X I L, X I R, A, XMU, TH, DEFM, SLOP  > 

•H 

*  COEFFICIENTS  OF  SMALL  MOTION  ACCELERATIONS  IN  LARGE  MOTION  DYNAMICS 

+  EQUATION  SUBROUTINE 

CALL  XLMMQN <  XMQN , A , XMU , XML , XLL , XMX , SLOP , DEFM , VV I , XX I ) 

* 

*  RIGHT-HAND  SIDE  FOR  LARGE  MOTION  DYNAMICS  EQUATION  SUBROUTINE 

* 

CALL  XLNFQ<FQ,U, XMQQP, DL1,UTH, ARTH, XIL,XIR,UO, HI 1,G,H21,URDD. . . . 
DL1D,UD,ARRDD,H41,TH,THD, DEFM, DEFMD, SLOP, SLOPD, A, XMU, XML, XLL, . . . 
TORQUE ) 

* 

*  LINK  STIFFNESS  MATRIX  SUBROUTINE 
CALL  SNKNCCKN, XK 1 1 , XMQQP, A, XMU, THD > 

* 

*  COEFFICIENTS  OF  LARGE  MOTION  ACCELERATION  IN  SMALL  MOTION 

*  DYNAMICS  EQUATIONS  SUBROUTINE 

* 

CALL  SMMNQ < XMNQ , DL 1 , NTH , X I L , DL 1 1 , DL 1 2 , U , TH , DEFM , SLOP , A , XMU , . . . 

XLL) 


*  RIGHT-HAND  SIDE  OF  SMALL  MOTION  DYNAMICS  EQUATIONS  SUBROUTINE 

* 

CALL  SMFN(FM,H21, W,G, URDD, DL  1  ,XIL, DL 1 !,DL 12, UD , DL 1D,H4 1,TH, . . . 

THD , DEFM , DEFMO , SLOP , SLOPD ) 

+ 

*  COEFFICIENTS  OF  SMALL  MOTION  ACCELERATIONS  IN  SMALL  MOTION  DYNAMICS 

*  EQUATIONS  SUBROUTINE 

* 

CALL  SMMNN ( XMNH , XMQQP , XML , A , XMU , XX I , YY I , XMX ) 

* 

*  ACCELERATION  COEFFICIENTS  MATRIX  AND  RIGHT-HAND  SIDE  UECTOR 

*  FORMULATION  SUBROUTINE 

* 

CALL  B I GFOR < B I GM , B I GF , XMQQ , XMQN , FQ , XMNQ , XMNH , XKN , FN , U ) 
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* 

*  LINEAR  EQUATION  SOLUEFi  FOR  ACCELERATIONS  SUBROUTINE 

* 

CALL  XLEQ ( B I GM , B I GF , SOL  ) 

* 

*  TRANSFORMATION  FROM  LOCAL  COORDINATE  TO  OLOBAL  COORDINATE  TIP 

*  POSITION  SUBROUTINE 
.+ 

CALL  GLOBCGPOS,  U,  OEFM ) 

*  INTEGRATE  ACCELERATIONS  AND  THEN  UELOCITV  TO  GET  LARGE  MOTION 

*  ANGULAR  POSITION  AND  SMALL  HOT  I  ON, LOCAL  COORD  I  NATE, T IP  POSITION 
+ 

DO  5  1=1,3 
SOL  1  < I >=$OL< I ) 

5  CONTINUE 
SORT 

UEL=INTGRL<UO,SOL 1,3) 

NOSORT 

THDC1 >=UEL<  1) 

DEFMD<  1  )=UELC2  ) 

SLOPO< 1 )=UEL<3> 

DO  10  1=1,3 
UELKI  )=UEL  < I  > 

10  CONTINUE 
SORT 

POS=  I  NTGFlL  <  POSO ,  DEL  1,3) 

NOSORT 

TH<  1  >=POS<  1 ) 

DEFM< 1 >=PQS(2) 

SL0Pa)=P0S(3) 

AC 1=S0L  1  <  1 ) 

RC2=S0L 1 <2 ) 

RC3=S0L1(3) 

UE  1=UEL<  1  > 

UE2=UEL(2) 

UE3=UEL<3) 

P01=P0S<  1) 

P02=POS<2) 

P03=P0S(3 ) 

XP0S=GP0S(2) 

VF'0S=GP0S<3> 

T0RK=T0RQUE<  1 ) 

LPD=PL ( 1 ) 

* 

*  OUTPUT  GLOBAL  COORDINATE  TIP  POSITION 

* 

TERMINAL 

*  ADAMS  METHOD  CHOSEN  DUE  TO  FASTER  COMPUTATIONAL  TIMES, SAME  ACCURACY 

METHOD  ADAMS 

PRINT  1 . OE-2, XPOS , YPOS 

SRUE  1.0E-3, XPOS, YPOS 

CONTROL  F I  NT  I M= 1 . 00 

GRAPH  <G1,DE=SPP.INT>  TIME, XPOS 

LABEL  <G 1 )  L0AD=2 . 115  KG 
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GRAPH  <G2.DE=SPRINT>  TinE.YPOS 
LABEL  (G2)  L0fiD=2. 115  KG  ' 

END 

STOP 

* 

*  LISTING  OF  SUBROUTINES 

* 

FORTRAN 


*  SUBROUTINE  XINIT  INTRODUCES  INITIAL  UALUES  AND  CONSTANTS  INTO  THE  MAIN 

*  BODY  OF  THE  PROGRAM 

* 

SUBROUT  I NE  X  INI T<TH, THD, DEFM , DEF MO , SLOP , SLOPD , UO , POSO, A , ML . MU, LL, 

•MR , E , 2 1 , PS , I FRAC , 10, CTM , UT , BE , DM , KU , TE , QL , PL , PL  I C ) 

REALMS  U0<3 ),  POSO <3  >,  ML .  MU,  LL ,  MR ,  TH ,  THD .  DEFM .  DEFI1D, SLOP,  SLOPD, 

•A.E.ZI, ITORQ,PS, I FRAC, l6,CTM,UT,BE,DM,K0,TE,QL,PL.PLIC. IMAX. 

•C 1 , C2,C3, C4, AP 1 , AP2, BETA  1 , BETA2 

I TORQ=  44 . 5540095 10000000 
DM=6. 22? ID-05 
TE=. 90000000000000 
PL=ITORQ/<DM+TE) 

PL  I C=PL 

CTI1-3. 70647720-13 

QL=CTM*PL 

KU=2 . 4029630-09 

PS= 1 . 37SSSD+07 

1 0=OL/<KU*DSC|RT  (PS-PL  > ) 

I MAX= 10 . 000000000000000 

* 

*  CURRENT  TO  ACTUATOR  REGULATED  BV  FRACTION  CHOSEN  IN  I FRAC 

* 

I  FFlRC= .  5D+0Q*  <  I  MAX- 1 0  ) 

UT =3.051270-04 
BE=690 . D6 
A=6.  177950-04 
ML =2 .  1 151000000000 
MU=786 1 . 05000000000000 
LL=0 . 99S50000000000 
MR=9. 000 1145 1000000 
E=2 .0011 
21=4.0650-10 
U0(  1  )=0 . 000000000000000 
U0<2  )=0 . 000000000000000 
U0<3 )=0 . 000000000000000 
POSO<  1  )=0 . 00000000000000 
POSO <2  )=- . 146064 14900000 
POSO (3  )=- . 2007S9S 1400000 
TH=POSO<  1 ) 

THD=UO<  1 ) 

DEFI1=P0S0(2) 

DEFMD=U0<2 ) 

SL0P=P0S0<3> 

SL0PD=U0<3> 
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RETURN 

END 


FUNCTION  SUBPROGRAM  ONE  COMPUTES  a  COEFFICIENT  OF  SHAPE  FUNCTION  MATRIX 

DOUBLE  PRECISION  FUNCTION  ONE(X) 

REAL *8  C1,C2, C3, C4 , A  IP, ASP, BETA  1 , BETAS 
COMMON /FCDATA/C 1 , C2, C3, C4 , A  IP, ASP, BETA  1 , BETAS 


ONE= 

9 

9 

9 


RETURN 

END 


C 1*(A IP* < COS < BETA 1*X )+COSH 
(BETA  1*X  >  >+  (S I  t'KBETA  1*X  >+S  I  NH<BETA  1*X  > )  >+ 
C2*(A2P*(C0S (BETAS*/ >+C0SH(BETA2*X  > >+ 
<S I NCBETA2*/ >+S I NH( BETAS*/ > ) ) 


FUNCTION  SUBPROGRAM  TWO  COMPUTES  a** 2 

DOUBLE  PRECISION  FUNCTION  TUOOO 
RERL*8  C 1 , C2 , C3 , C4 , A 1 P , A2P , BET A 1 , BETAS 
COMMON /FCDATA/C 1 , C2, 03,04, A  IP, ASP, BETA  1 , BETAS 

TWO=  <  C  t*  <  A 1 P* < COS  ( BETA  1 *X )+COSH 

*  (BETA 1*X ) >+<S I N(BETA1*X )+S I NHCBETA 1*X  > ) )+ 

*  C2* < ASP* <  COS  <  BET A2*X HCOSH < BET A2*X  > >+ 

«  (SIN( EETA2*X )+S I NH ( BETR2*X ) ) ) >**2 

RETURN 
END 


FUNCTION  SUBPROGRAM  THREE  COMPUTES  b  COEFFICIENT  OF  SHAPE  FUNCTION  MATRIX 

DOUBLE  PRECISION  FUNCTION  THREE (X) 

REAL*8  C 1 , C2, C3, C4 , A  IP, A2P , BETA  1 , BETA2 
COMMON /FCDATA/C 1, C2, C3,C4, A  IP, ASP, BETA  1 , BETAS 

THREE*  C3*(A  1  F'*(  COS  (  BETA  1*X  HCOSH 

«  (BETA  1*X  >  >+ ( S IN ( BETA  1 *X )+S I NH ( BETA  1 *X >  > )+ 

*  C4* ( ASP* ( COS <  BET A2*X >+COSH  < BETAS*/  )  )+ 

«  ( S I N ( BETA2+X )+S I NH ( BETAS*/ >  > ) 

RETURN 

END 


FUNCTION  SUBPROGRAM  FOUR  COMPUTES  b**2 

DOUBLE  PRECISION  FUNCTION  FOUR(X) 

REAL*8  C 1 , C2 , C3 . C4 , A 1 P , A2P , BETA  1 , BETAS 
COMMON /FCDATA/C 1 , C2, C3, C4, A  IP, ASP, BETA  1 , BETAS 

FOUR* 


# 


(C3*(R  IP* (COS (BETA 1*X )+COSH 
( BETA  1 *X ) )+ ( S I H( BET A 1 *X >+S I NH ( BETA 1*X ) ) )+ 
C4* ( ASP* ( COS ( BETAS*/  )+COSH( BETAS*/  )  )+ 


n 


(S  I  IKBETfi2*.X  >+S  I  NH<BETfl2*X ) ) )  >**2 


RETURN 

END 


FUNCTION  SUBPROORhN  FIUE  COMPUTES  PRODUCT  ob 


DOUBLE  PRECISION  FUNCTION  FIOECX) 

REFILLS  C 1 ,  C2 ,  C3  .C4.fi  IF' .  fi2P ,  BETA  1 .  BETR2 
COMMON /FCDRTfi/C 1 . C2 , C3, C4, fl IP , fi2P , BETfi 1 . BETR2 


F I  UE= 

# 

# 

# 

n 

# 

8 

RETURN 

END 


<C  1+<fi  1F'*< COS  (BETA  1*X  )+COSH 
(BETA 1*X > >+($ INCBETft 1+X >+S I NHCBETfi 1*X>) >+ 
C2* < fi2P* <  COS ( BETA2*X J+COSH < BETR2+X ) )+ 

<8 1 N<BETfi2*X >+S I MH<BETfi2*X >  > ) ) 
+(C3*(fi IP* (COS (BETfi 1+X )+COSH 
<  BETA  1: *X  >  >+  <  S I N  <  BETA  1  *X  )+S  I MH  (  BETA  1  *X  » )+ 
C4*< fl2P* <  COS  <  BETA2+X >+C0SH(BETR2+X ) )+ 
<S  I  N<BETR2+X  )+S  I  MH<BETfl2*X  >  ) ) ) 


FUNCTION  SUBPROORAM  SIX  COMPUTES  -a  LL  -  a  x 


DOUBLE  PRECISION  FUNCTION  SIX(X) 

REALMS  C 1 , C2 , C3 , C4 , A 1 P , A2P , BETfi 1 , BETA2 
COMMON /FCOfiT A /C 1 , C2,C3, 04, A  IP,  fl2P, BETA  1, BETA2 

S I  X=  .  9985*<C  1*(R  IP* (COS (BETA  1*X  J+COSH 

*  (BETfi 1+X  >  >+ (S I H< BETfi 1+X )+S I MH (BETfi 1+X ) ) )+ 

«  C2* ( fi2P* <  COS C  BETR2+X J+COSH < BETR2+X ) )+ 

*  (SI N<BETR2*X >+S I NH(BETfi2*X >  >  >  >+ 

*  X+CC 1+<R 1F+(C0S<BETR  1*XHCQSH 

*  <  BETfi 1 *X  > )+ <  S I N  <  BETfi 1 *X  >+S I NH  <  BETfi 1 *X  >  >  >+ 

*  C2* <  fi2P+  < COS  <  BETft2*X >+C0SH<BETfl2*X  >  >+ 

»  (SI N<BETfi2*X  )+S I NH(BETfi2*X > > >  > 

RETURN 

END 


FUNCTION  SUBPROGRRM  SEUEI1  COMPUTES  -b  LL  -  b  x 


8 

tt 

8 

9 


DOUBLE  PRECISION  FUNCTION  SEUEN(X) 

REfiL+8  C 1 , C2 , C3, 04 , fi IP, R2P , BETfi 1 , BETA2 
COMMON /FCDflTfl/C 1 , C2, 03, 04, fi IP, R2P, BETfi 1 , BETR2 

SEUEH=  .9985*  (C3*(fi1P*(C0S(BETfi1*X)+C0SH 

(BETR  1*X?)+(SI NCBETR  1*X )+S I HH< BETfi 1*X ) ) >+ 
C4+ ( R2P* ( COS  <  BETfi2*X )+C0SH(BETft2*X ) )+ 
<S I N<BETft2*X >+S I NH(BETR2*X >  >  >  > 
+X* ( C3* <  fl  1 P*  < COS ( BETfi 1 *X >+COSH 


(BETB1+.X)  >+<SIN(BETfl1*X  >+S  I  NH ( BETA  1  *X  )  > )+ 
C4*(A2P*<CGS(BETR2*X >+C0SH(BETA2*X > >+ 
(S I N(BETA2*X )+S I NH(6ETA2*X >  »  > 

RETURN 

END 


*  FUNCTION  SUBPROGRAM  EIGHT  COMPUTES  COEFFICIENT  OF  u"  **2 


DOUBLE  PRECISION  FUNCTION  EIGHT (X ) 

REALMS  C 1 , C2, C3, C4, R IP, A2P, BETA  1 , BETR2 
COMMON /FCDflTfi /C 1 , C2 , C3 , C4 , A  IP , A2P , BETA  1 , BETA2 

E I GHT=  <C  1+BETA  1*BETR  1*(R IP* < -COS 

*  (BETA 1*X  )+COSH(BETR  1*X  >  >+<-S I N(BETA 1*X )+S I NH(BETR 1*X ) ) >+ 

*  C2*BETR2+BETR2* ( A2P*  < -COS  <  BET R2+X >+C0SH(BETA2*X )  )+ 

*  ( -S I N (  BETR2+X )+S I NH < BETR2*X > ) >  >+*2 
RETURN 

END 

* 

* . 

*  FUNCTION  SUBPROGRAM  NINE  COMPUTES  COEFF I C i ENT  OF  v" 


DOUBLE  PRECISION  FUNCTION  NINE(X) 

REALMS  Cl, 02,03,04, A  IP, A2P, BETA  1 , BETA2 
COMMON /FCDRTR/C 1 , C2 , C3, C4 , A 1 P , A2P , BETA  1 , BETR2 


* 

* 

* 

* 


NINE= 

# 

# 

# 

it 

# 

# 

# 

RETURN 

END 


(C 1*BETft 1*BETR 1*(R 1P*<-C0S 

(BETA  1+X  )+COSH(BETA  1*X  >  )+<-S  I IKBETA  1*X  )+S  I  NH(BETA  1*X  )  > )+ 
C2*BETR2*BETR2*  <  A2P*  <  -COS  (  BETfl2*X  HC0SH<BETfl2*X  >  >+ 

<-S I N(BETR2+X >+S I NH<BETA2*X )))) 
*<C3*BETR 1*BETA 1*(A IP* (-COS 

(BETA  1*X  >+COSH(BETR  1*X  >  )+<-S  I  IKBETA  1*X  MS  I NH-BETA  1*X  )  > )+ 


C4*BET A2+BETA2*  ( R2P* <  -COS  ( BETR2*X  )+COSH  ( BETR2*X  )  M 

( -S I N ( BETR2+X MS I NH ( BETA2+X ) > > ) 


FUNCTION  SUBPROGRAM  TEN  COMPUTES  COEFFICIENT  OF  v"  **2 


DOUBLE  PRECISION  FUNCTION  TEN(X) 

REALMS  C 1 ,  C2 ,  C3 , 04 ,  A 1 P ,  A2P ,  BET  A 1 ,  BETA2 
COMMON /FCDATA/C 1 , C2, C3, C4 , A  IP, A2P, BETA  1 , BETA2 

TEN=  (C3*BETR 1+BETR 1*(fi IP* (-COS 

*  (BETA  1+X  MCGSH(BETfi 1+X ) )+(-S I  IKBETA 1+X >+S I NH(BETA 1+X ) ) M 

*  C4*BET A2+BET R2+ < R2P* < -COS ( BET A2*X MCOSH( BETR2*X > )+ 

*  <-SIN< BETR2+X )+S I NH ( BETR2+X ) > ) M+2 
RETURN 

END 
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SUBROUTINE  FORM  COMPUTES  COMPONENTS  OF  MATRICES 


SUBROUT  I  HE  FORM  (  14 ,  NTH ,  WD ,  DL 1 ,  DL  1 D ,  X I L ,  X I R ,  RRTH ,  14RDD ,  ARRDD .  U,  UD , 
«XMOGP, G, H 11 ,H2 1 ,DL 1 1 . DL  12 , H4  1 . XK 11, fl, HU . ML, LL , TH , THD . DEFM, DEFMD, 
*SLOP, SLOPO, E, Z I , MR, MX, VV I , XX I > 

REfiL+8  14 (3,3),  NTH < 3 , 3 > , NO (3,3),  DL  I < 3 , 3 > , OL  10 < 3 , 3 ) , X I L ( 3 , 3 > 

RERL+8  XIR(3,3),RRTH(3,3),MRDB(3,3),RRR0D(3,3),U(2,  1  ),U0<2,  1) 

RERL*8  XMOQP ( 2 . 2 ) . 0(3 .  1 ) . H 1 1  < 1 . 3 ), H2 1 <2 . 3 ) . DL 11 <3 .3 ) . DL 12(3, 3 > 

REAL+8  H4 1( 1 , 3),XK 11(2,2), MU, ML,LL, MR, MX, TH 

RERL*8  THD, DEFM, DEFMD, SLOP, SLOPO, XX I . VV 1 , fi . E , Z I 

RERL*8  C 1 , C2, C3, C4, fl IP, A2P, BETR 1 , BETR2 

REAL+3  ONE . TNO , THREE , FOUR , F I UE . E I GHT , N I  HE , TEH . V 

EXTERNAL  ONE , TNO , THREE , FOUR , F I UE , E I OHT , N I  HE , TEN 

N(  1, 1 )=  1 . 00000000000000 

N( 1 . 2 )=0 . 00000000000000 


N( 1 , 3  >=0 . 00000000000000 
N(2, 1 )=LL*DCOS(TH) 


N(2,2)=0C0S(TH) 

N(2,3)=-DSIN(TH) 

N(3, 1 )=LL*DS I N(TH) 

14(3, 2  )=DS  I  H(TH  ) 
N(3,3)=0C0S(TH) 

NTH( 1 , 1 >=0 . 00000000000000 
NTH( 1 , 2 )=Q . 00000000000000 
NTH( 1 . 3 )=0 . 00000000000000 
NTH (2 . 1 >=-LL*DS I N(TH ) 
14TH(2, 2  )=-DS  I  N(TH  ) 

NTH(2, 3  >=-DCQS(TH ) 

UTH(3, 1  >=LL*BCOS(TH> 

NTH (3 , 2 )=DCOS( TH ) 
NTH(3,3)=-0SIN(TH) 

ND(  1, 1 )=0 . 00000000000000 
ND(  1 , 2 )=0 . 00000000000000 
ND< 1 . 3 )=0 . 00000000000000 
140(2^,  1  )=-LL:f'DS  I  N(TH  )*THD 
NO ( 2 . 2 )=-DS I N ( TH )*THD 
140(2,3  >=-OCOS(TH)+THD 
ND(3,  1  )=LL*DCOS(TH  )*THD 


ND  < 3 , 2  )=DCOS ( TH )+THD 
N0(3,3)=-DSIN(TH)*THQ 
DL 1 ( 1 , 1 )= 1 . 00000000000000 
OL 1 < 1 , 2 )=0 . 00000000000000 
DL  1  (  1 , 3 )=0 . 00000000000000 
DL  1  (2,  1  )=0 . 00000000000000 


OL 1 (2 , 2 )= 1 . 00000000000000 
DL 1(2,3)=-SL0P 
DL  1(3,  1  >=DEFM 
OL 1 (3,2)=SL0P 
DL  1  (3, 3  )=  1 . 00000000000000 
DL 10(  1 , 1 )=0 . 00000000000000 
DL  1D( 1 , 2 )=0 . 00000000000000 
OL  1D(  1 , 3  )=0 . 00000000000000 
DL ID (2,  1  >=0 . 00000000000000 
DL  10(2,2 )=G . OOOOOOOOOOOOO 


DL  10(2 , 3  >=-SL0F‘D 

OL 10(3, 1 >=DEFMD 

OL 10(3, 2  >=SLOPO 

DL  10(3, 3 >=0.00000000000000 

X I L  (  1 ,  1  )=t1L 

X I L ( 1 ,2 >=0 . 0 1678545900000 
X I  L(  1 , 3  >= . 00000000000000 
X I L  < 2 , 1 >=0 . 0 1 678545900000 
X I L(2, 2 >=1.7767920-04 
X I L ( 2 , 3 >=0 . 00000000000000 
X I L<3,  1  >=. 00000000000000 
X I L<3, 2  >=0 . 00000000000000 

X  IKS,  3  >=2.98679 10-03 
MX= . 0 1678545900000 

XX I  =  1.7767900-04 
VV I =2. 98679 10-03 

X I  R(  1,  1  >=11R 

X I R ( 1,2 >=0 . 000000000000000 
X I R  < 1 , 3  >-0 . 000000000000000 
XIR(2,  1  >=0.000000000000000 
X I R  <  2 , 2 )= . 027467 130000000 
X I R(2, 3  >=0 . 000000000000000 
X I R <3 , 1 )=0 . 000000000000000 
X I R  < 3  2  >— 0 . 000000000000000 
X I  F;(3, 3  >= . 027467 13000000 
RRTH( 1, 1 >=0.00000000000000 
fiRTH( 1 , 2  >=0 . 00000000000000 
RRTH( 1 , 3  >=0 . 00000000000000 
RRTH(2,  1  >=0.0000000000i9000 
fifiTH ( 2 , 2  >=-DS I H ( TH  > 

RRTH( 2, 3  >=-DC0S<TH > 

RRTH(3, 1 >=0 . 00000000000000 
RRTH (3,2  >=DCOS ( TH  > 

RRTH( 3 , 3  >=-DS I N(TH  > 

UROD< 1 .  1  >=0 . 00000000000000 
WRDD( 1 , 2  >=0 . 00000000000000 
URDD(  1 , 3  >=G . GGGGOOOOGGGGOO 
14R0D<2,  1  >=-LL*DCOS(TH  )*<TH0**2  > 
WROD (2,2  >=-0C0S (TH  >* ( TH0**2 > 
UR0D(2, 3 >=0S I H(TH >#(THD++2 > 
14RDD(3,  1  >=-LL*DS  I  M(TH  )+<TH0*+2  > 
URDD (3.2 >=-0S I M ( TH  >*< THD**2 > 
URDD (3, 3  >=-OCOS(TH >*(THD**2 > 
RRRDD( 1, 1 >=0.00000000000000 
RRRDOC 1 ’ 2  >=0 . GGGGGGGGOOGOOG 
RRRDO (1,3  >=0 . 00000000000000 
RF;R0D<2,  1  >=0.00000000000000 
RRRDO (2,2  >=-0008 <  TH  >* <  THD**2 > 
RRF;00(2, 3  >=08 1  N(TH  >*(THD**2  > 
RRRDO (3 . 1 >=0 . 00000000000000 
RRR00C3, 2  >=-0S I N(TH >*<TH0**2 > 
RF.RDD  (3, 3  >=-DCOS  ( TH  >*  (THD++2  > 
U(1,  1  >=DEFH 
0(2,  1  >=SLOP 
UDC  1,  1  >=DEFMD 
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UD<2, 1 >=SLOPD 

CALL  0QG4(-LL.0.D0,TU0,Y) 

XMC!QP(  1 ,  1  )=Y 

CALL  DQG4(-LL,0.D0,FIUE,Y> 
XMQQP( 1 , 2 )=V 
XMQQP(2, 1 >=V 

CALL  DQG4 (-LL,G. DO , FOUR , V > 

XMQQP(2,2)=V 

G<1.1  )=0 . 00000000000000 

0(2,  1  )=0 . 00000000000000 

6(3 .  1  >=-9 . 30660000000000 

H1 1(  1,  1  )=4. 3565 1900000000 

H 1  K  1 , 2  )=-2 . 42325369300000 

H 1 1 ( 1 , 3 )=G . 00000000000000 

H2 1 ( 1 ,  1  )=0 . 00000000000000 

H2 1 ( 1 , 2 )=0 . 00000000000000 

CALL  DQ04 ( -LL , -0 . DO , ONE  ,V> 

H21(1,3)=A*MU*Y 

H21<2, 1 )=G. 00000000000000 

H2 1  <2, 2 )=0 . 00000000000000 

CALL  DQG4 (-LL, 0.00, THREE, V) 

H21(2,3)=A+MU+Y 

DO  50  1=1,3 

DO  60  0=1,3 

DL  1 1  < 1 , 0  )=0 . 00000000000000 
DL  12( I , J )=0 . 00000000000000 
60  CONTI  HUE' 

50  CONTINUE 

DL  1 1  (3,  1  >= 1 . 00000000000000 
DL  12 (2 , 3  )=- 1 . 00000000000000 
DL  12(3,2)= 1 . 00000000000000 
H4 1  (  1 ,  1  )=I1L 

H4 1  ( 1 , 2  )=0 . 0 16735459000000 
H4 1  (  1 . 3  >= . 000000000000000000 
CALL  DQG4(-LL . 0 . DO, E I GHT, V ) 
XK  11(1,  1  )=V*E*Z 1 
CALL  DQG4(-LL ,0 . DO, HI  HE, V ) 

XK 1 1 ( 1 , 2 )=Y*E*Z I 

XK  1 1(2, 1)=V*E*ZI 

CALL  DQG4 (-LL , 0 . DO , TEN  ,V) 

XK11(2.2)=V+E+ZI 

RETURN 

END 


+ 

4: 

+ 


SUBROUTINE  XLHMQQ  COMPUTES  THE  COEFFICIENTS  OF  LARGE  NOTION  ACCELERATION 
IN  THE  LARGE  MOTION  DYNAMICS  EQUATION 


SUBROUT  I  HE  XLMflQQ  ( NQQ ,  U ,  XMQQP .  DL  1 ,  UTH ,  ARTH ,  X I L ,  X I R ,  A ,  MU .  TH ,  DEFN . 
•SLOP) 

REALMS  MQO, UT( 1 , 2 ), P<  1 , 2 ) . DL 1T(3 . 3 ), WTHT<3, 3 >, ARTHT(3, 3 ), P 1(3, 3 ) 
RERL^S  P2(3,3),P3(3,3),P4(3,3),P5(3,3),P6(3,3),P7(3,3),MU 
REAL *8  U(2, 1 ),XMQQP(2,2),DL 1(3,3), UTH(3,3), ARTH(3,3),XIL(3, 3) 
REAL +8  X I  R(3 , 3  ) .  A .  TH .  0EFI1 .  SLOP ,  SP ,  TP 
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4: 

* 

+ 

* 

* 


* 

* 

* 

* 


L=1 

N=3 

MQQ=0 . 00000000000000 
CRLL  TRRNS(U,UT,M,L) 

CALL  MATMULCUT, XMQQP, L, M ,M,P) 

CALL  HRTMUL <P,U,L,M,L,SP) 

CALL  TRANSCDL 1 , DL IT,  N,  N ) 

CALL  TRAMS <ARTH . ARTHT ,  N,  M  ) 

CALL  TRAMS ( UTH , WTHT ,  N . M ) 

CALL  MflTMUL ( UTH ,  DL  1 , M . M , H , P 1 ) 

CALL  MflTMUL <P1,XIL,N,N,N.P2) 

CALL  HRTMUL ( P2 , OL  V T  , M , M , M , P3 ) 

CALL  MflTMUL  <  P3 ,  l-JTHT .  H ,  H ,  M ,  P4  ) 

CALL  MflTMUL <fifiTH,XIR,N,N,N,P5) 

CALL  MflTMUL  < P5 , ARTHT , H , M , M , P6 ) 

CALL  MAT ADD <P4,P6,N, M , P7 ) 

CALL  TRACE <P?,N, TP > 

MQQ=«1./3.  )*A*MU)  +  <fl*MU*SP>  +  TP 

RETURN 

END 


SUBROUT  I  ME. XLMMQM  COMPUTES  THE  SMALL  MOTIOM  ACCELERATIONS  IN  THE  LRRGE 
IN  THE  LARGE  MOTION  DYNAMICS  EQUATION 

SUBROUT  I  ME  XLMMQM  C  XI1QN .  A ,  MU .  ML .  LL ,  MX ,  SLOP .  DEFM ,  VY I  .  XX I  ) 

REALMS  XI1QN<  1 , 2),  MU,  ML, LL,  MX,  A,  SLOP,  DEFM,  YV I ,  XX I 
REALMS  C 1 , C2, C3, C4, A  IP, A2P, BETA  1 , BETA2, Y, S I X, SEUEH 
EXTERNAL  SIX,SEUEN 
CALL  DQG4(-LL,0.DQ,SIX  ,Y) 

XMQN<  1, 1 )=  <  A+MU+V )+  <  ML*LL )+MX 
CALL  DQG4 < -LL , 0 . DO , SEMEN , V > 

XMQN  <  1 , 2  )=  <  fl+MU+Y  )+  ( MX+LL  )+ VY  I +XX I 

RETURN 

END 


SUBROUTINE  XLMFQ  FORMULATES  THE  RIGHT  HAND  SIDE  FOR  THE  LARGE  MOTION 
DYNAMICS  EQUATION 


SUBROUT  I NE  XLMFQ <FQ,U , XMQQP , DL 1 , UTH, ARTH, X I L, X I R, UD, H 11 , 6, H2 1 . 
#URDO, DL  ID, HD, RRRDD . H4 1, TH, THD, DEFM, DEFMD, SLOP , SLOPD, A, MU, ML, LL, 
•TORQUE  ) 


REALMS 

REAL+S 

REALMS 

REALMS 

REflL+S 

REALMS 

REAL+S 

REALMS 

REALMS 

M=2 


FQP(2,2),F’<  1 , 2>,  F  1<  1,3),P2(  1,3),  P3<  l,3),P4<3,3),P5<3,3) 
P6<3,3),P?<3,3),F'8(3,3),PQ(3,3),P10<3,3),P1 1(  1,3),P12(  1,3) 
FPFH<3,3  >, FHP<3,3  ),FPT<3,3), DL IDT  <3, 3 ), HOT (3,3), ARRDDT  <3, 3) 
UT< 1 , 2 ), DL 1T<3, 3  >, HRDDT (3, 3),  FPF03, 3 ), FPS<3, 3  >, UTHT <3, 3 ) 
U<2,  1  ),XMQQP02,2),DL  1(3,3),UTH<3,3),ARTH<3,3),XIL<3,3) 
XIR(3,3),U0<2, 1 ),H 1 1 ( 1,3), G<3, 1 ),H2 1<2 , 3). HRDD<3,3 ) 

DL  1D(3,3),MD<3,3),RRRDD(3,3),H4 1(  1,3),ilU.LL,l1L 
A, TORQUE . FQ, TH, THD, DEFM, DEFMD, SLOP . SLOPD 
FP, SP, TP, TFP, FTHP 


L=  1 
N=3 
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CALL  TRANS <U.UT,M,L> 

DO  10  1  =  1,2 
DO  20  J-1.2 

fqpc  i ,  j  >=xnqqpc  I ,  J  >*THD 
20  CONTINUE 
10  CONTINUE 

CALL  NATNUL ( UT , FQP , L , M , N , P ) 

CALL  MATHUL  C P , UO , L , M , L . FP ) 

CALL  TRANS C NTH , NTHT ,  N,N> 

CALL  NATNUL  <  H 1 1 , NTHT . L . N , N ,  P 1 ) 

CALL  MATMULCP 1 ,  G,  L , N , L, SP  > 

CALL  MATHUL < UT  ’  H2 1, L '  M,H,P2) 

CALL  NATNUL ( P2 , UTHT , L , N , N , PS ) 

CALL  MATMUL  CPS ,  G ,  L ,  N ,  L ,  TP  > 

CALL  TFiANSCDL  1 ,  DL  IT , N,  N  ) 

CALL  TRANS < NFlDD,  UROOT ,  N,  N  ) 

CALL  NATNUL < NTH ,DL1,N,N,N,P4> 

CALL  MRTMUL  C P4 , X I L , N , N , N , P5  > 

CAL L  NATNUL  <  P5 , DL 1 T , N , N , N . PC  > 

CALL  NATNUL  <  PC , NRDDT , N , N , N ,  FPF  > 

CALL  TRANSCDL 1D,DL 1DT,N,N) 

CALL  T RANS  C  ND , NOT ,  N ,  N ) 

CALL  MATNULCNTH, DL I , N, N, N, P? ) 

CALL  NATNUL (P?,XIL,N,H,N.P8> 

CALL  NATNUL  C PS , DL 1 DT , N , N  ‘  N . P9 ) 

CALL  NATNUL  <  P9 . NOT . N . N . N . FPS ) 

CALL  TRANS CARROD, ARRDDT, N, N > 

CALL  NATNUL CARTH, X I R,  N, N, N, P 10  ) 

CALL  NATNUL  CP  10 , ARRDDT , N . N, N, FPT > 

DO  30  1=1,3 
DO  40  J= 1 , 3 
FPSC I , J )=FPS< I , J  >*2 . 

40  CONTINUE 

30  CONTINUE 

CALL  NAT ADD  < FPF , FPS . N , N , FPFH > 

CALL  NATADDCFPFH, FPT ,N,N, FHP > 

CALL  TRACE C FHP, N.TFP) 

CALL  NATNUL CH4 1 , DL IT, L, N, N, P 1 1 ) 

CALL  NATNUL  <  P 1 1 , UTHT , L , N , N , P 1 2 ) 

CALL  NATNUL CP  12, G, L, N, L, FTHP > 

FQ=  C -2 . *fl*NU*FP )  +  SP  +  TP  -  TFP  +  FTHP  +  TORQUE 

RETURN 

END 


SUBROUTINE  SMKN  CALCULATES  THE  LINK  STIFFNESS  MATRIX 

SUBROUT  I  HE  SMKN C  XKN , XK 1 1 , XMQQP , A , MU , THD ) 

REALMS  XKNC2, 2  >,  KNPC2, 2  ) .  XNQQPC2, 2  ) ,  XK  1 1 C2, 2  ) .  A,  THD,  I1U 
DO  10  1=1,2 
DO  20  J=  1 ,2 

KNPC  I ,  J  )=XNQQPC  I ,  J  )+C-A  )+-NU*CTHD**2  ) 

XKNC I , J >=KNPC I , J  >+XK 1 1 C I , J  > 

20  CONT I  HUE 
10  CONTINUE 
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* 

* 

+ 

* 


* 

* 


* 

+ 

* 


RETURN 

END 


SUBROUTINE  SMHNQ  COMPUTES  THE  COEFFICIENTS  OF  LftROE  MOTION  RCCELERRT I  ON 
IN  THE  SMALL  MOTION  DYNAMICS  EQUATION 


SUBROUT  I  ME  SMMNQ (  XMNQ . DL 1 , NTH , X I L , DL 1 1 . DL 1 2 , U , TH , DEFI1 , SLOP , A , MU , 
**LL  > 

REALMS  XMNQ (2 . 1 > . DL  12T(3 . 3 ) . DL  1 1T<3, 3 ) . UT<3, 3 ) . P 1 <3 . 3 ) . P2(3 . 3 ) 

RERL*8  P3<3, 3) . P4<3 , 3 > , F'5(3 , 3  >, P6<3 . 3  >, DL 1 <3, 3 ), WTH<3 . 3  > . X I  L<3, 3 ) 

RERL*3  U < 3 , 3  ) , DL 1 K  3 , 3 > , DL  1 2 (  3 , 3  > , TH , DEFM , SLOP , fl , MU , LL 

REAL *3  C 1 . C2 , C3 , C4 , A  IP , A2P , BETA  1 , BETA2 

REALMS  SIX,SEUEN,Y 

EXTERNAL  SIX, SEMEN 

M=2 


L=  1 
N=3 

CALL  TRANS <DL 1 1 ,  DL  1 1T .  N, N > 

CRLL  TRRHS(DL12,DH2T,H,N) 

CALL  TRANS  OJ.I4T,N.N> 

CALL  MATMUL (WTH, DL  1 , N, N, N, P 1 ) 
CALL  MATMUL <F'1,XIL,N,N,N, P2 > 
CALL  MATMUL  (  P2 ,  DL  1 1 T ,  N ,  N ,  N ,  PS ) 
CALL  MATMUL  ( PS , NT , N . M , M , P4  ) 
CRLL  TRACE <P4,N,TFP1> 

CALL  DQG4(-LL, O.OO, SIX  .Y> 
XMMQC  1 ,  1  >=TFP  1  +  <Y*R:t:MU  > 

CALL  MATMUL (P2.DL12T,N,N, M , P5 ) 
CALL  MATMUL (P5, WT , N , N, N , P5  > 
CALL  TRACE  <  PO , N , TFP2  > 

CRLL  DQG4 <-LL, 0 . 00, SEUEM . V > 
XMMQ(2, 1 )=TFP2  +  (Y+fi+MU) 
RETURN 
END 


SUBROUTINE  SMFN  CALCULATES  THE  RIGHT  HAND  SIDE  OF  THE  SMALL  MOTION 
DYNAMICS  EQUATION 


SUBROUT  I  ME  SMFN  <  FN , H2 1 . W , G , WRDD , DL 1 . X I L . DL 1 1 , DL 1 2 , HD . DL 1 D . H4 1 , TH , 
#THD,DEFI1,  DEFND .  SLOP,  SLOPD  ) 

REAL *8  FN<2, 1  ),P  1  <2 . 3), P2<3, 3 > . P3<3. 3 >,P4<3, 3), P5<3 , 3 ) . P6<3 . 3 ) 
RERL*8  P?(3,3),P8(3,3>,P9(3,3),P10(3,3),P11<3,3),P12<3,3),P13<3,3) 
REALMS  P14<1.3),P15<1.3),P16<1.3),P17(1,3),TP<2,  O.FPC2,  1),SP(2,  1) 
REALMS  FN  1<3, 3 ) , FN2<3 , 3 >, G<3, 1 >, H2 1(2,3), URD0<3,3 ), DL  10(3, 3 ) 

REALMS  UD(3,3), H4 1 (1 , 3 ), X I L<3, 3),  I4<3, 3 >,  DL 1 1 <3, 3 >, DL 12(3,3 ) 

REAL+8  DL  1(3,3), DL 1 1T (3,3), DL 12T <3, 3) ,  UT (3,3) 

REALMS  TH ,  THD,  DEF11,  DEFMD,  SLOP .  SLOPD 

11=2 

L=  1 

N=3 


CALL  TARNS (U,UT,N,N) 

CALL  MATMUL  <H2 1 , MT , M , H, N , P 1 > 
CALL  MATMUL <P1,G,M,N,L,FP> 
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CfiLL  TRflNSCOL 1 1 , DL 1 1T . N , N  > 

CfiLL  TFlftNSCDL  12,  DL  12T .  N .  N  ) 

CfiLL  MfiTMUL ( URDD ,  DL  1 , N ,N,N,P2> 

CRLL  MFITMUL  <P2 ,  X I L ,  N ,  fl ,  N ,  P3 ) 

CfiLL  MfiTMUL  CP3 . DL 1 1 T , N , H , N , P4 ) 

CfiLL  MfiTMUL <  P4 ) NT , H , N . M , P5 > 

CfiLL  MfiTMUL  < MD , DL 1 D , M , M , H , P6 > 

CfiLL  MfiTMUL < P6 , X I L , M , M , H . P? ) 

CfiLL  MfiTMUL (P?,DL1 1T,M.M,M,P3> 

CfiLL  MfiTMUL  (PS .  W,H,  H .  H .  P9  ) 

CfiLL  MfiTMUL <P3, DL 12T , H , H . M . P 10  > 

CfiLL  MflTMULCP 10, MT, H, M, M, P 1 1 ) 

CfiLL  MfiTMUL ( P? , DL  1 2T , H , H , N , P 1 2 > 

CfiLL  MfiTMUL  <P 12 , UT , H , M , H , P 1 3 > 

DO  10  1  =  1,3 
00  20  J=1 .3 
P9( I , J  >=  P9< I , J )*2 . 

P  13<  I ,  J  )=F'  13<  I ,  J  )*2 . 

20  CONTINUE 
10  CONTINUE 

CfiLL  MfiTfiOO(P5 ,  F'Q,  N .  N ,  FN  1 ) 

CfiLL  MfiTflDO  <  P 11 ,  P 1 3 ,  N ,  N ,  FN2  > 

CfiLL  TRflCECFN 1 , N, TFN 1 > 

CfiLL  TRACE ( FN2 , N , TFN2 ) 

SP(1, 1 >=TFN 1 
SP<2.  1  >=TFN2 

CfiLL  MfiTMUL <H4 1 ,  DL  1 1T, L,  N, N, P 14  > 
CRLL  MfiTMUL (F  14, NT, L, H, N, P 15 ) 

CALL  MfiTMUL  <  P 1 5 , G . L , N , L , FN3 ) 

CfiLL  MfiTMUL  <  H4 1 , DL 12T , L , N , N , P 1 6  > 
CfiLL  MfiTMUL <P16,NT,L,N,N,P 17) 

CfiLL  MfiTMUL ( P 1 7 , G , L , N , L , FN4 ) 

TP( 1 , 1 )=FN3 
TP<2, 1 >=FN4 
DO  31  1=1,2 

FIK  1 ,  1  >=FP<  1,1)  -  SP(  1,1)  +  TP<  1,1) 
31  CONTINUE 
RETURN 
END 


+  SUBROUTINE  SI1I1NN  COMPUTES  SMALL  MOTION  ACCELERflT  I  OHS  IN  THE  SMALL  MOTION 

*  DVNfitl  I CS  EQUfiT  I  ON 

* 

SUBROUT  I  HE  SI1MNN  (  XI1NN ,  XMOOP ,  I1L  .  fi .  MU ,  XX I ,  VV I ,  MX ) 

REfiL*8  XMNH  <  2 , 2  > ,  XI10QP  <  2 , 2  ) ,  ML ,  MU ,  fi ,  XX 1 ,  VV  I ,  MX 
DO  10  1  =  1,2 
DO  20  J= 1 , 2 

XI1M1K  I  .  J  )=0 . 00000000000000 
20  CONTINUE 
10  CONTINUE 

XMNIK  1 ,  1  )=I1L 
XMNN< 1 , 2 )=MX 
XMHHC2, 1 >=MX 
XMNN<2, 2  >=XX I +VV I 
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DO  30  1=1,2 
DO  40  J=  1 , 2 

XI1MH  <  I ,  J  >=XNNN<  I ,  J  >  +  XMQQP<  I  ,  J  >*A*NU 
40  CONTINUE 
30  COHT I NUE 
RETUP, H 
EHD 


*  MULTIPLICATION  SUBROUTINE 

4: 

SUBROUT  I NE  MRTMUL <fl ,  B ,  II,  L ,  M ,  C  > 

REflL*8  ACM, L),B(L,N), CCM , N ) 

DO  10  1=1, M 
DO  20  J=  1 , N 
C<l,J>=0.0 
DO  30  I NDEX=  1 , L 

C< I , J >=C< I ,  J  >  +  R(  I ,  I MDEX >*B< I NDEX, J ) 
30  CONTINUE 
20  CONTINUE 
10  CONTINUE 
RETURN 
END 


SUBROUTINE  TRANS  PERFORMS  TRANSPOSE  OPERATION 

SUBROUT  I NE  TRANS (A, B, N, L  ) 

REAL +8  ACM,L  >,BCL,M) 

DO  10  1=1, M 
DO  20  J=  1 ,  L 
BCJ,  I )=A< I , J > 

20  CONTINUE 
10  CONTINUE 
RETURN 
END 


SUBROUTINE  TRACE  CALCULATES  THE  TRACE  OF  A  MATRIX 

SUBROUT  I NE  TRACE <  A . M , TRAC  ) 

REAL*8  A(M,M) 

TRAC=0 . 0 
DO  10  1  =  1,11 
TRRC=TRRC  +  AC  I, I  ) 

10  CONTINUE 
RETURN 
END 


MATRIX  ADDITION  SUBROUTINE 

SUBROUT  I NE  MAT ADD ( A , B , M , L . C ) 
REAL *8  ACM,L ),B<M,L),C<M,L > 
DO  10  1  =  1,  M 
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DO  20  J=1,L 

CCI,J)=ACI,J>  +  B<I,J) 
20  com  I  HUE 

10  CONTINUE 
RETURN 
END 


*  SUBROUTINE  BIOFOR 

* 

SUBROUT  I NE  B I GFOR < B I GM , B I GF , MQQ , XHQN , FQ , XMNQ . XMNN , XKN , FN , U ) 

RERL*B  B I  OIK 3, 3 ), B I GF<3 , 1 ), XMQN< 1,2), XMHQ<2, 1 ), XMNN <2, 2), XKN (2, 2 ) 
REAL +8  FNC2, 1),U<2, 1). MQQ, PCS, 1),FG 
11=2 
L=  1 

BIGMC  1,  1  )=MQQ 
BIGMC 1,2>=XMQN< 1,  1) 

B I GIK  1 , 3  )=XI1QNC  1.2) 

BIGMC2,  1  >=XMNQ< 1, 1> 

B I  GIK  2 , 2  )=XMNH  C  1 ,  1) 

B I GMC2, 3 )=XMNN( 1,2) 

B 1 011(3,  1  )=XMNQ(2,  1) 

B I  GM  <3,2  )=XI1NN  <  2 ,  1) 

B I  GM  <  3 , 3  )=X‘NNN  (2,2) 

BIGFC1,  1  )=FC] 

CALL  MATMUL  <  XKN ,  U ,  N .  11 ,  L ,  P  ) 

BIGFC2, 1 >=FH< 1, 1 )-P< 1,1) 

BIGFC3,  1  )=FNC2, 1)-P(2, 1) 

RETURN 

END 


*  SUBROUTINE  XLEQ  CALL  II1SL  SUBROUTINE  LEQT2F  TO  SOLUE  SYSTEM  OF  EQUATION 

* 

SUBROUT  I NE  XLEQ < B I GM , B I GF , SOL > 

REALMS  BIGMC3,3),BIGF<3,  1  ),S0LC3),IJKARERC  IS) 

M=  1 
N=3 

CALL  LEQT2F  < B I  GM ,  M ,  N ,  N ,  B I  GF ,  M ,  WKAFiE  A ,  I ER ) 

DO  10  1=1,3 
SOL  < I >=B I GF ( 1 ,  1) 

10  CONTINUE 
RETURN 
END 


*  SUBROUTINE  GLOB  PERFORMS  TRANSFORMATION  FROM  RELRTIUE  POSITION  TO 

*  GLOBAL  POSITION 
+ 

SUBROUT  I  NE  GLOB  <  GF'OS ,  U ,  DEFH ) 

REAL *8  GPOS C  3 ),  IK 3, 3 ) , DEFM , RL < 3 ) 

RL<  1>=1.000 
RLC2)=0.0D0 
RLC3)=DEFM 
M=3 
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L=  1 

CALL  MflTMUL <U . RL , N, N , L ,  GPOS ) 

RETURN 

END 


* 

* 

* 

■¥ 

* 

+ 

* 

* 

+ 

* 

* 

* 

* 

* 

* 

* 


* 


SUBROUTINE  0QG4  < I MSL  SUBROUTINE) 

FURPOSE 

TO  COMPUTE  INTEGRAL <FCT<X>,  SUMMED  OVER  X  FROM  XL  TO  XU) 


USAGE 

CALL  0Q04  <XL,XU,FCT,Y> 

PARAMETER  FCT  REQUIRES  AN  EXTERNAL  STATEMENT 
DESCRIPTION  OF  PARAMETERS 

XL  -  DOUBLE  PRECISION  LOWER  BOUND  OF  THE  INTERVAL. 

XU  -  DOUBLE  PRECISION  UPPER  BOUND  OF  THE  INTERVAL. 

FCT  -  THE  NAME  OF  AN  EXTERNAL  DOUBLE  PRECISION  FUNCTION 
SUBPROGRAM  USED. 

V  -  THE  RESULTING  DOUBLE  PRECISION  INTEGRAL  UALUE . 

METHOD 

EVALUATION  IS  DONE  BV  MEANS  OF  4-POINT  GAUSS  QUADRATURE 
FORMULA.  WHICH  INTEGRATES  POLVNOMIALS  UP  TO  DEGREE  ? 
EXACTLY.  FOR  REFERENCE,  SEE 

V . I . KRYLOV ,  APPROXIMATE  CALCULATION  OF  INTEGRALS, 
MACMILLAN,  NEW  YORK /LONDON,  1002,  PP.  100-111  AND  337-340. 

SUBROUT  I NE  DQG4 < XL , XU, FCT , Y ) 

DOUBLE  PRECISION  XL, XU, Y, A, B,C,FCT 
ft= . 500*<XU+XL > 

B=XU-XL 

C= . 430568 1 557970262900*6 

Y=.  17392742256872693D0*<FCT <R+C  >+FCT< A-C  >  > 

C=.  16999052 1792428 1300*B 

V=B*<Y+ . 32607257743 1 2730700* < FCT (A+C )+FCT<fl-C ) ) ) 

RETURN 

END 
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APPENDIX  C 


DATA  REDUCTION  ALGORITHM 

The  accelerometer  senses  an  global  acceleration,  we  will  call  it 
S,  since  motion  in  the  vertical  plane  involves  both  X  and  Y 

components  of  acceleration.  In  order  to  obtain  tip  position 
information  the  global  transformation  matrix  can  be  used  to  to 
obtain  an  expression  for  v(0).  This  is  then  integrated  twice  using  a 
DSL  simulation  program.  The  value  of  v(0)  is  then  placed  in  the 
global  transformation  matrix  to  obtain  an  expression  for  tip  position 
in  terms  of  the  global  coordinate  system.  The  process  is  repeated 
below. 

Using  the  global  transformation  matrix  we  obtain  a  value  for 
X  and  y  in  terms  of  v . 


x  =  L  cos  ©  -  v  sin  ©  (C-1) 

y  =  v  cos  ©  +  L  sin  ©  (C-2) 

these  expressions  are  then  differentiated, 

x  =  -L  sin  ©  ©  -  v  sin  ©  -  v  cos  ©  ©  (03) 

y  =  -v  sin  ©  ©  +  v  cos  ©  +  L  ©  cos  ©  (04) 

these  expressions  are  differentiated  once  again, 
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X 


=  -L82  -  L  8  sin  8  +  v  82  sin  8  -  v  8  cos  8  -v  8  cos  8 


-  v  8  cos  8  -v  sin  8  (C-5) 

y  =  -L  82  sin  8  +  L  8  cos  8  -  v  82  cos  8  -  v  8  sin  8 

-  v  8  sin  8  -v  8  sin  8  +  v  cos  8  IC-6) 


This-  expression  can  be  simplified  since  for  our  model  8  =  0,  8  = 


constant,  L  =  constant. 


(C-?) 


x  =  vl  -sin  8  )  +  v  (  -28  cos  8  )  +  v  1  82  sin  8  )  -  (  L  82  cos  8  ] 
y  =  v(  cos  8  )  +  v  {  -28  sin  8  )  +  v  (  -82  cos  8  )  -  (  L  82  sin  8  ) 

(08) 


now  express  in  terms  of  V, 


v  -  2  82  v  v  +  4  92  v  v 


+  84  v  v  +  4  L  Oz  v 


(09) 

L  2  84  -  S  2  =  0 


tuhere 


this  expression  is  coded  in  a  DSL  program  and  (C-9)  is  solved 
explicitly  for  V  and  integrated  twice.  This  value  of  V  is  then 
placed  in  expressions  (C-1)  and  (02)  and  global  tip  position  is 


determined . 


93 


LIST  OF  REFERENCES 


1.  Everett,  H.  R.,  "Robotics  in  the  Navy  Part  11:  Nonindustrial 
Development  Efforts",  Robotics  Age  The  Journal  of  Intelligent 
Machines,  v.7,  pp.  11-14,  December  1985. 

2.  Roth,  B. ,  "Robots-State  of  the  Art  in  Regard  to  Mechanism 
Theory, "  Journal  of  Mechanism.  Transmissions,  and  Automation 
in  Design,  v.105,  pp.  11-12,  March  1983. 

3.  Schmitz,  E.,  Experiments  on  the  End-Point  Position  Control  of 
a  Very  Flexible  One-Link  Manipulator.  Ph.D.  Thesis,  Stanford 
University  Dept,  of  Aeronautics  and  Astronautics,  June  1985. 

4.  Usoro,P.B.,  and  Nadira,  R.,  "Analysis  of  Lightweight  Flexible 
Manipulator  Dynamics",  ASME  Proceedings  of  International 
Computers  in  Engineering  Conference  and  Exhibit,  pp.  167- 
174,  1984. 

5.  Chang,  L.,  Dynamic  Analysis  of  Robotic  Manipulators  with 
Flexible  Links.  Ph.D.  Thesis,  Purdue  University,  December 
1984. 

6.  Book,  W.J.,  "Analysis  of  Massless  Elastic  Chains  with  Servo 
Controlled  Joints,  ASME  Journal  of  Dynamic  Systems, 
Measurement,  and  Control,  v.101,  pp.  187-192,  September 
1979 

7.  Sunada,  W.H.,  and  Dubowsky,  S. ,  "On  the  Dynamic  Analysis 
and  Behavior  of  Industrial  R.obotic  Manipulators  with  Elastic 
Members",  ASME  Journal  of  Mechanisms.  Transmissions,  and 
Automation  in  Design,  v.105,  pp.  42-51,  March  1983. 

8.  Book,  W.  J.,  "Recursive  Lagrangian  Dynamics  of  Flexible 
Manipulator  Arms",  The  International  Journal  of  Robotic 
Research,  v.3,  Fall,  1984. 


94 


9.  Hastings,  G.,  and  Book,  W.,  "Experiments  in  the  Control  of  a 

Flexible  Robot  Arm",  Proceedings. _ R0B0T9  Conference,  v.2, 

Rl/SME,  pp.  20-45 — 20-47,  June  1985. 

10.  Truckenbrodt,  A.,  "Modelling  and  Control  of  Flexible 
Manipulator  Structures",  Proceedings  of  4TH  CISM-lFToMM 
Svmp.  on  Theory  and  Practice  of  Robots  and 

Manipulators. Warsaw,  pp.  110-120,  1981. 

11.  Cannon,  R.,  and  Schmitz,  E. ,  "Initial  Experiments  on  the 
End-Point  Control  of  a  Flexible  One-Link  Robot",  The 
International  Journal  of  Robotic  Research,  v.3,  Fall,  1984. 

12.  Petroka,  R.  P.,  Computer  Simulation  And  Experimental 
Validation  Of  A  Dynamic  Model  (Equivalent  Rigid  Link  System) 

On  A  Single-Link  Flexible  Manipulator.  M.S.  Thesis,  Naval 
Postgraduate  School,  Monterey,  California,  October  1982. 

13.  MOOG  Series  760  Two-Stage  Flow  Control  Servovalve  Catalog. 

MOOG  INC.,  East  Aurora,  New  York. 

14.  Merritt,  H.E.,  Hydraulic  Controls  Systems.  Wiley,  New  York, 
1967. 

15.  MacADIOS  Model  411  Hardware  Unit  Users  Manual.  GWI 
Instruments,  Cambridge,  Massachusets 


95 


INITIAL  DISTRIBUTION  LIST 


No,  Copies 

1.  Defense  Technical  Information  Center  2 

Cameron  Station 

Alexandria,  Virginia  22304-6145 

2.  Library,  Code  0142  2 

Naval  Postgraduate  School 

Monterey,  California  93943-5002 

3.  Department  Chairman,  Code  69Hy  1 

Department  of  Mechanical  Engineering 

Naval  Postgraduate  School 
Monterey,  California  93943 

4.  Professor  Liang-Wey  Chang,  Code  69Ck  2 

Department  of  Mechanical  Engineering 

Naval  Postgraduate  School 
Monterey,  California  93943 

5.  Naval  Surface  Weapons  Center,  White  Oak  1 

Code  R402  (ATTN:  Mary  Lacey) 

New  Hampshire  Avenue 
Silver  Spring,  Maryland  20910 

6.  LT  Kevin  P.  Gannon  5 

2000  Swan  Terrace 

Alexandria,  Virginia  22307 


96 


DUBLIN  ' 


•vir^  im?  ar.Y 


NAVAL  POO-' 


.  B. 


i.'J  b 


•  :iOOL 


iThes 
G143{ 
c.l  i 


Thesis 

G14395 

c.l 


22101, 


Gannon 

Modeling  and  experimen¬ 
tal  validation  of  a 
single-link  flexible  mani- 
pulator. 


10  HAT  83 


3  2  2  9  6 


221012 

Thesis 

G14395  Gannon 

^  Modeling  and  experimen- 

tal  validation  of  a 
single-link  flexible  mani¬ 
pulator. 


